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ABSTRACT 


The tecnnique of Extended Kalman filtering is applied 
to a passive single sensor location and tracking problem. 
Signal angle of arrival and doppler shifted frequency are 
used as observations for two different filter formulations: 
one for fixed X-Y coordinates of position and velocity and 
one with an orthogonal system rotated to align an axis with 
the target heading. The fixed X-Y system was found to give 
better tracking performance on the Monte Carlo simulation. 
A new method of calculating the initial covariance matrix 
was extended to reformulate the Kalman filter.equations for 
a nonlinear problem. The use of the full initial covariance 
matrix as opposed to only the diagonal elements, provides 
better transient response and lower error variances in the 


simulation. 
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i INTRODUCTION 


There are many engineering ЖОО Ст in nonlinear filter- 
ing which do not have the Luxury of many observations over 
an extended period of time. The estimator or filter 15 
required to provide a rapid, accurate solution before steady 
state conditions of the gains and covariance of error are 
reached. In these situations it is imperative to use as 
much information as possible from the observations, the 
known apriori conditions, and the statistical relationship 
of the initial filter states. 

The location of acoustic emitters by remote passive 
sensors is such a problem. The received acoustic signals 
may be processed to obtain noisy angle of arrival and 
noisy doppler shifted frequency data. These, in turn, can 
be used as observables to provide location and tracxing 
information on the emitting target. The ability to accom- 
plish this with a single sensor (sonobuoy) as well as the 
flexibility to incorporate additional sensors as available 
would be highly desirable for the Anti-Submarine Warfare 
ВОИ БУ ee filltersusime angle of arrival and doppier- 
shifted frequency can be applied to other tracking problems 
иене radar comaín as well. 

There are presently available methods which use only 
the doppler-shifted frequency from some array of three or 


more sensors [46]. In general, they require a good initial 
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guess as to where the emitter is located because they rely 
оп a linearization method closely resembling a least-square 
fit of the received frequency data. Currently, research 

is going on to incorporate angle and frequency data in a 
common arrangement [35]. 

A first objective of this research was to apply non- 
linear filtering theory to the single sensor location and 
tracking problem and develop an algorithm to solve this 
estimation problem. The calculation of initial filter 
States uSing a Single sensor with angle and frequency measure- 
ments only, represents a difficult task. A single sensor 
making either measurement alone does not provide а unique 
solution and it is only when the combination is used that 
.a single determination of the target position is possible. 
The geometry of the problem, while simple, does not make 
determination of initial position using the specified obser- 
vations very simple. Yet, this initial position estimate 
must be chosen so that the subsequent linearized filter 
techniques will converge. In doing so, the effect of various 
choices which are available to the filter designer became 
evident. The primary choice is that of a system model, 
and even for a simple constant-velocity target, differences 
in the model formulation are possible. The’ value of the 
initial filter states and their M anit uncertainty, as 
Tel leeted in the initial covariance matrix, are seen to 


play important roles in the time it takes for the filter 


d 





to converge. Using all of the covariance terms in this 
matrix, as opposed to only the diagonal variances, results 
in better filter performance in the transient phase and 
lower error variances for a specified track duration. The 
geometric placement of sensors was considered to determine 
where they might be placed for best filter performance. 
It is also important to determine how well the filter is 
performing while in operation, not so much as a quantitative 
measure of accuracy, but whether the filter is indeed esti- 
mating a reasonable track. Following the lead of Periot 
[34], the filter residuals were found to be a good measure 
of a valid track. 

In Chapter Two general filtering theory is considered 
with the emphasis on the underlying probability structure 
ot пе опб1та1 filters Ehis approach clearly pornts out 
the simplicity of a linear-Gaussian system and its resultant 
filtering solution as well as the difficulties and complexi- 
ties of the nonlinear problem. The continuous formulation 
of the filtering problem is also reviewed with reference 
to stochastic integrals and the determination of the strictly 
formas OluLionweeln thneswtanal section of Chapter Iwo, the 
techniques that various authors have used to approximate 
the optimal nonlinear solution will be presented. The 
approaches range from a direct calculation of the aposteriorl 
denssey function [10], to a simple linearization of state 


and observation equations about some nominal state trajectory [38] 





Chapter Three describes in detail the single sensor 
passive location and tracking problem and will develop the 
two state formulations. The equations for an Extended 
Kalman Filter for each case are presented. The important 
initial condition equations are derived and the initial 
covariance of error expressions for the states are found. 
The technique used to obtain the initial covariance matrix 
iS mew and consists of a direct calculation of the changes 
in the nonlinear initialization equations when the input 
measurements change by one standard deviation. The values 
for the filter state excitation matrix (which turn out to be 
state dependent), are presented next. The ability to add 
additional sensors or observables to the filter is an 
important advantage of the Extended Kalman method and this 
is developed next. 

Chapter Four contains a new approach to the determina- 
tion of error covariances for nonlinear functions of random 
variables. This approach is an extension of the method 
Ugekstoschtain the initial covarlance Of state error a 
The method may be readily applied to formulate the complete 
nonlinear filtering problem using the linear Kalman filter 
equations. The one step prediction and updating is accom- 
pidshHed by a direct calculation of the change in the поп- 
linear state or observation function caused by a one-standard-- 
deviation change in each state variable. The matrix of the 
correlation coeffictents of the states is found to play an 


portant role. 
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A Monte Carlo simulation of the two filters developed 
in Chapter Three is given in Chapter Five for a wide range 
of problem geometry. The effects of initial conditions 
and initial covariance matrices wi11 be seen to play an 
important role in performance of the filter. 

The final chapter summarizes the an of this inves- 
tigation and presents the conclusions Рес аа for 


further study. 
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DIE PTLTERTNG THEORY 


РОНЕ КРОВА Бит ТЕС APPROACH FOR THR DISCRETE CASE 

Since the extraction of states or parameters from nolsy 
measurements is fundamentally a problem of estimating random 
variables, it is important to consider the underlying prob- 
ability theory of a general filtering problem. Indeed the 
characterization of the estimates with the appropriate prob- 
ability density function is the most complete knowledge that 
can be obtained about these estimates. The evolution of this 
density function in time, and its change with incoming measure- 
ments, represents the general solution to the estimation prob- 
lem. How the value for the estimate is determined will be 
governed by the cost or penalty charged for making a wrong 
choice [31,44]. 

| Assume a discrete system of the following form 


x(ct1) ва (хо) жк) + у уу; (2.1) 


if 


ӨК = ВХ, КЛ + v(k) (2:12) 


with x the vector of states or parameters (or both) to be 
estimated and z the vector of observations. The notation used 
ИЕЗПЕПОШ Гре text is that vector quantities "are lower case 
letters anc underlined and matrices are given by upper case 
letters. The initial states x(0) are assumed to be from some 
initial density p(x(0)) and the statistics of the system and 


measurement noise w and v are known and given by 
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ЕГи(к)] = 0 
Е[у(к)] = 0 
Elw(k)+v(3)] = 0 ODE NL T 
ELg(x (c) ,k) wc) · (ебх(к) „к) сме )) Ј = 9006), 
ЕГу(к) Y (3)] = В) .6 
with 
б = 1 к=] 
=0 kj, 
and 
БГи(к)-х (0)] = 0 
ЕГу(к)-х (0)] = 0 (2.3) 


Note that the above system of equations defines a Markov 
process; that is, a process whose probability law for future 
times depends only on the present state values. In terms of 
the density functions 


ec НИ A 0)) = р(х(к+1)/х(к) (2.40 


The Markov property of a process is very fundamental and 
along with the uncorrelated noise assumptions written above, 
allows great simplification in the recursive expressions 
developed later. 

Let 


Z% = (z(k),z(k-1),...,z(1)) (2.5) 


be the set of all measurements on the system up to the 
present time. The probabilistice knowledge one has about the 
system, given the set of measurements Z^ is contained in the 


2 


о robabillity density funetion of the states given 





the measurements: p(x(3J)/zE). Note that the index value 
j and superscript k are not necessarily the same. This 
allows the three main classes of problems — smoothing rer 
j<xk, filtering for j = k, and prediction for j > k = to 
be considered simultaneously. 
I The Cost Function 

Let the error of the estimate be att = x(j) - x(j) 
where x(j) is the true state value at time t(j) and x(j) 15 
the value of the estimate. Let L(e(j)) be a cost function. 


The value of the estimate is chosen so as to minimize the 


expected value of the cost function: 


x(j) is the value of x(j) such that E[L(e(j))] is minimun, 
with 


E[L(e(3))] = $ Lle(3))p(x(3)/2%) ax(3) 


This expectation is taken over the conditional density 
function of the states and theréin lies the importance of 
mobs! РИС ОМ 

Т аре Ро тези гее main cost tunetions which lead 


to different choices for the estimate. 
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Cost Function L(e(j)) Name Estimate x(j) 


P e? ()-e(3) minimum variance Conditional 
or minimum mean mean 
square error 


2. |е())| minimum absolute Conditional 
его median 
Sr a е) < ô uniform Conditional mode 


or maximum 
a posteriori 
estimate 


1 otherwise 


TABLE I. Cost Functions and Their Estimates 


2. Recursive Bavesian Formulation 
In the discrete formulation of the filtering problen, 
the conditional density function can be written in a recursive 
manner with the initial condition probability distribution 
as the starting function [26]. 
Let the conditional density of the filter states be 


р(х(к)/2*). Using Bayes rule this can be written as: 


k-1 
РК таса а алы” 
р(х(к)/2^) = р(х(к)/ш(к), 2-1) = — k 
Pz ыы 
р(2(к)/х(к), 2071) + р(х(к), 277) 
р(а(к)/2 91). Dez 
| MATA AS 
p(x(k)/2*) = E Е p(xao/zk- 1) (o 
р(2(К)/4 
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Thus Бре current value of the опа Попа density function 
is found from the predicted density p(x(k) / 25-1) and a 
weighting term which depends on the current observation. 
Since the denominator term does not depend on the states, 
x(k), it can be looked on as a normalizing constant[10]. 
The predicted conditional density function сап be 


found from the density function one step back in the following 


manner: 
p(xü) / z*bs [f р(х(к), х(к-1) / 205 
x(k-1) 
-— p (x(k) x (k-1) 2871) 

руши = Л кт дх(к-1) 

x(k-1) p(Z2" 7) 

B. px /x(e-2),2*7) p(x(k-1),277) aca) 

x(k-1 тоқы (2.8) 
Now using the Markov property of the state system, 
plx(x)/2 5d) = J р(х(к)/х(к-1)) p(x(-1)/2%) ax(k-1) 

Eu |. (2.9) 


ШОО ла олор the integral indicates that the integration 
is performed over all permissible values of the states one 
step back. The predicted conditional density is thus found 


from the density one step back and involves a spatial 
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integration over all values of alli the states at that time. 


The final recursive expression for the conditional density 


is thus 

p(x(k}/2*) = 

aa 7 x(k )) | T" 

к Lye 5 PERA MARY "ейейхек-1) 

p(z(k)/Z x(k-1) | 
2.10 


with the condition p(x(0)/20) = pi OA Ten dl dn ail 
conditional density functions above are obtained from tne 
probability density functions of the state excitation noise, 
w(k-1), and the observation noise, v(k). 

Let the density of w(k) be p, (w(x)) and the density 


of v(k) be p,(v(k). Therefore 


p(z())/x00) = py (200) - h(x(k),k)) , (2.11) 
and p | 
р(х(к)/х(к-1)) = р [Е >(х(к-1),к-1).[х(®)-Ё(х(к-1),к-1)1] 
(2.29 


3. Linear Gaussian Case 
When the state equations and the observation 
equations are linear, and the initial density of x(0) and 
the densities of w(k-1) and v(k) are Gaussian all of the 


above eonditional densities turn out to be Gaussian since 
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they involve linear operations on Gaussianly distributed 
random variables. Since these densities are completely 
determined by two quantities, the vector mean and the 
covariance matrix, the recursive expression for the condi- 
tional density function, Eq. (2.10) simplifies to two 
PemoLedmexpresisions: опе for the vector mean and the other 
for the covariance matrix. These two expressions, when 
rewritten are the well-known Kalman filter equations [36, 
пара, 

When the state or observation equations are nonlinear 
or the noise statistics are non-Gaussian, the above simpli- 
fication is not valid. Thus the entire density is needed 


bo conpletely solve the problem. 


Bo ТНЕ CONTINUOUS CASE 
The equations for the states and observations can be 


Viet Ре ла continuous form 


and 


dar АСО ООВ ау) к: са. Ша) 


The above equations are Ito stochastic differential 


equations and must be manipulated using the rules of Ito 


Eu uS TITI The difficulty with ordinary calculus is in 


2n 








Pnesdetinielon of "integrals involving white noisé- The 
first equation above is to be interpreted as 
t t 
ҒАЗАЛ Se At.) , S a(t). tachi | EEE) Ka E 
E : (2.15) 

Since the iast integral involves integration over random 
increments (dw(t)), with random quantities in the integral, 
it is not even defined by ordinary calculus [21]. The 
Ineremenbssaptearing in Equations (2.13) and (2.14), Auch) 
and dv(t) are increments of two independent Wiener processes. 

When the continuous equations are interpreted in the 
Ito sense, the process Бар still defines a Markov process 
and as sed evolution of the density function of x(t) is 
given by the Kolmogorov equations or the Fokker-Planck 
equation mo cu Kushner [25], Bucy [6], and Fisher [18] 
have shown that the conditional density P(x(t)/Z") - where 
25 is the set of observations in the interval (t 2) - also 
obeys a Kolmogorov type equation which is modified by the 
observations. The result is an Ito differential equation. 
for the conditional density which is strictly a formal 
solution to the problem. By this is meant that no closed 


form solution can be found because there is no theory To 


solve the equation [21]. See also Bucy and Joseph [8]. 


C. SOLUTION METHODS AND APPROXIMATIONS 
cano surprise. ana thes work 1s no exteption, that 


осле practical non-linear filters use the Kalman 
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filter equations in some linearized system of equations 
[38]. The linearization is usually carried out about some 
nominal state trajectory or about a predicted value for the 
states on a step by step Қ This technique is Extended 
Kalman filtering. The tacit assumption here is that with 
Gaussian noise, there is some region in state space around 
the true state values where the system response is linear 
and therefore Gaussian. Similarly the observation errors 
will be linear and Gaussian. The greatest difficulty with 
this approach is insuring that the linearization is valid; 
especially at the initial filter estimates where the errors 
may be quite large. For the passive problem described in 
the next chapter, the initial conditions are not known and 
must be obtained from the first few observations with suffi- 
cient accuracy to insure filter convergence. 

Several authors have looked at higher order terms in 
the expansions of the state and observations equations 
[3,5,27,40,43]. This greatly increases the complexity of 
the calculations and has not been shown to be generally 
apolicable to all problems. 

There have been attempts to look at higher moments of 
КО ШООЛА топла! density function itself іп an effort to 
characterize the function at each state point. Fisher [18] 
has derived equations for the evolution oí higher order 
moments. The difficulty is in determining how many moments 


are needed to describe the density and in solving the highly 


eo 





coupled set of nonlinear difference or differential equations 
Thich are the result [24]. 

Srinivasan [41] and Sorensen and Stubberuds [39] expanded 
the density into an othogonal series and then looked at the 
evolution of each of the terms in the expansion. It is 
difficult to know how many terms to incorporate and in some 
cases the series may not converge at all [21]. Spline 
function expansions have been used but have the same inherent 
сет съ етес ГІЛ), Swirling, et al. [42] have әшфбепрвей а 
logarithmic expansion of the conditional mean. 

One of the most successful expansion techniques has 
been the Gaussian-Sum Expansion of Alspach and Sorenson 
[1,2]. While the computational burden is quite large, 
this method is one of the few that will soive multi-modal 
distributions. The primary reason for its universal appli- 
cation is that each linearization of the state and observa- 
tion equation only has to be valid about the mean of each 
of the assumed Gaussian distributions used in the sum. 

Bach term in the sum is then filtered using the linear 
Kalman filter equations, and the results recombined and 
normalized to give an approximation to the conditional 
density. 

For limited order systems, Bucy and Senne [7] have done 
Ed reci lion of the conditional density using the 
Bayes law recursive equations developed in the previous sec- 


tion. Here again the computational burden is enormous and 
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is only possible with two or three state variables. For 
these systems, however, this technique does result in the 
complete solution to the nonlinear filtering problem in the 
Sense that tne result is the complete aposteriori condi- 
tional density function. The particular value of the state 
estimate may then be determined using a suitable cost cri- 
terion. See also Senne [37] and Bucy, Hecht and Senne [9]. 

The method of least squares, whilé being strictly a 
non-probabilistic approach is widely used in non-linear 
problems [4,12,20,28,33]. In this method a non-probabilistic 
cost function is developed which is usually a quadratic form 
of the state trajectory with some appropriate state model 
(See for example Sage and Melisa [36]). This cost function 
is then minimized, using the observed data, with respect 
to the state trajectory points. As new data is added a 
recursive algorithm can be used to update the estimates 
[15,30]. With appropriate weighting matrices used in the 
Boastsiumetion, this) method can be shown to bé exactly equi- 
valent for Gaussian noise statistics to the maximum a pos- 
teriori estimate using a probability approach [21,36]. 

The method of least squares for nonlinear problems is 
usually solved by linearization about some estimate and 
ПРЕ овас том илс 1 ща solution is founds Thus the initial 
guess must be sufficiently close to allow for convergence. 
Also for a larre-order system the amount of computation may 


be extremely large. 
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The technique involves setting up a suitable cost 
function as a set of simultaneous non-linear equations 
which contains the observed data and some set of state 
parameters as unknowns. In this system of equations 
(usually over-determined) the "best fit" criteria is applied, 
generally with a data error norm. This method is used in 
the doppler-frequency-only localization method [46]. -When 
the system of over-determined equations is linear, this 
approach results in the pseudo-inverse matrix technique 
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ТТТ. PASSIVE LOCATION AND TRACKING 


In the two-dimensional location problem, there are two 
position and two velocity components which will specify a 
constant-speed, constant-heading target. Since the dcppler 
shifted frequency is used as an observable, it is also 
necessary to estimate the rest frequency of the emitter. 
These five quantities will be used to define two different 
filter formulations: -one in which a target independent X-Y 
coordinate frame is used and one in wnich a target dependent 
system with one axis aligned with the estimated heading is 
used. Before presenting the two filter types, a general 


discussion of the Extended Kalman filter is given. 


А. EXTENDED KALMAN FILTER 
Consider a nonlinear discrete system of state and 


observation equations given by 
ху ix Ck yk) + в(х(Кк),К)и(к) 03 
and 


zc) Mk) A e) e 2) 


ік еһесе еппасіотізеі, e and hadre nonlinear functions of 
the state variables x, w(k) is plant excitation noise, and 
v(k) is measurement noise. The plant noise and measurement 
noise are assumed uncorrelated, zero-mean, and white. That 
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Elw(k) + wi(J)] = Q'(k) 84, 


and 


| 


E[v(k)* v'(j)] = RK) 8 


kJ 


In order to apply the linear filter equations, (3.1) 
ana (3.2) are expanded about the best estimate of the state 
at that time and only the first-order terms are kept. 


Equation (3.1) gives 


СИЕ оО) (к) ET (c) mu) , | (3.3) 


паше 


Ф(к) = 


% Q2 
Joa} |e 


x-x(k) 


Similarly Eq. (3.2) yields 


АСК) =н) x(k) t v(k) | (3.4) 
where 
ah 
H(k) = p =x! (I) 2 aoa 


tn 


x(k) is the estimated state value after the k" measurement 


Is the predictor value of thefstate before the 


kN measurement. That 13, 


х' (к) = f(x(k-1),k-1). 
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A state error vectór is defined by 


MTS Eo 


and a predicted state error vector is defined by 
х' (к) = x'(k) - x(k) 
The covariance of state error matrix is defined by 


PO юре CO пи) 


and the predicted covariance of state error matrix is 


given by 


P'(k) 9 E[x' (X) - x' 00] 


! 


pon А 
The state excitation matrix is given by AT 


_ ——— cá o 
2 


Q(k) 7» E[T(k)-w(O. + ик) г , 
| | 


a 
— — ЕЕЕ 


— 


and the measurement noise covariance matrix is 


Н(к) = Е[у(К) - у (к)] 





M Е 
V 


Ne 


р 
-- 
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The Kalman Filter equations are given by [38] 


2) P* (+1) = Ф(к)Р(к)Ф (к) * Q(X) (3.5a) 
б(х) = Р'(к)Н Qo [ROO P ! GORT (X) € ROO T. (3.5b) 
Р(к) = {т - 8008007 P OO (3.50) 
X' (k) = f(x(k-1),k) е; (3.5а) 
2! (к) = h(x'(k),k) (3.58) 
x(k) = x'(k) + с(к)Га(к) - а1(81 . (3.5 Г) 


The Q matrix serves not only to allow Гог maneuvering 
Бис 2150 tofaccount: for any model inaccuracies. That 15, 
any discrepancies between the true action of the physical 
тет and its characterization by Ea. (3.3). For a filter 
which reaches steady-state conditions the Q also serves to 
prevent the gain matrix G(k) from approaching zero by always 
insuring uncertainty in the predicted covariance of error 
matrix P'(k). 

ШІ. Х-тү Filter Equations 

Figure 3.2. Gives the а of the states used for 

the X-Y filter. For a constant-course, constant-speed target, 
the plant state equations can be described as two second order 
systems, one for X and one for каната а state for Пе 650 


frequency, Bo: These are 





4 Target (x,y) 





V 
y 
STATES 
Xx 
Sensor (0,0) y 
Vx 
V 
y 
Fo 


Fig. 3-1 x-y Filter Geometry 


31 





Ер | / ‘ • En 
x (k*1) xar Tv IF TE Y Pd 





ку (уе ото 20) | 
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xiki) z = | (3227 
Vy Oct) АЛА РОСА 


n. sc) а А 


where x(k), and y(k) are the position coordinates at time 
OE v, OX) and V, (c) are the X and Y components of velocity, 
T is the time between observation, i.e., t(k+1) - t(k), and 
(К) is the rest frequency of the emitter. 

The excitation terms f4 through fg areBinetuceadELe 
account for the random changes in speed and heading and Fo 
which can occur for a maneuvering target. The quantities 
Ве ти апа Ve. aro= the randemeachanges olf the сацвес литре 
are assumed to be independent, zero mean, plecewise-constant 


rates of change. They have variances defined by 


О • 2 = Ely: 27 
V 
S S 
2 2 
0% = Ely ] 
5 5 
ER 
Ор Е ELY A =: 
О O 
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The values for the standard deviations were taken from 


typical maneuvering parameters for the target; 


е 


ілес, — 
у“ 


| | р 
Se 10 kts/1000 sec 
S 
d 
сһ = .5 Hz/1000 sec. | e faee 
O 


The effect of this excitation is to increase the predicted 
value of the covariance of the state error matrix. 


Writing the equations in linear state form results 
in C Грас па 


/ Ex v7 


ми 
х(к+1) = 6 x(k) * T w(k) Ке“ (3:20) 


where: 


ps 
+3 
e 
po 
O 


0 0 0 0 
o т o 

A 0 

0 0 0 0 | 

апа | 
2 
T 

| > О 0 0 

0 ШІ 0 0 0 

| 2 

0 о т P: - 
Г = | 

ol 0 0 A 

0 0 0 0 
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The vactor w(k) represents themertect on the states of the 
random excitations and may be calculated from the equations 
relating target X and Y velocity to the target heading and 


velocity. The*X velocity is 


E CIRO | (3.8) 


which when differentiated gives 


> у 
: X 
mB = v*.cos В. - у «sin 6 .Үб 7 —AÀAE* - у Ye 
ШЕ 5 5 5 6 dis, У 9s 
A 
A ғас“ 
(3.9) 
Similarly 
ie за 8”. (3,10) 
and 
54 Vy 
y = "s T Va’ YÈ o (3.11) 
5 5 5 
The frequency term is just 
p Y$ (3.12) 
O 
Thus 
р Vx y 
WC аи аг а Т тҮ. у ПБ ] 
S D e S O 
(3.13) 
where from the assumption on tne y's 
Е[м(х)] =0 . (3.210) 
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Meme ота co variance Matrix iS thus found from 


a 


1 1 
д = Е и(к) кк) гр. ¿To (3.15) 
p 1 | 7 По ч | 
- | 2 p (у. кет зи." ; г? ж 
Let СХ Ж” / e : А2 E IE Е { 
Pm 2 2 2 
MEE Nc Е Vy 9% T (3.162) 
o S D 
2 _у)2 2 2 2 | 
о“ = ( с: + vi" Gs | (3.160) 
y ШЫ V. x За 
а © 
И 2 
E з-с; 05. (3.166) 
ху = 5 


where the states are evaluated at the current state estimates, 


x(k). Substituting these expressions in the Q matrix results 
in | 
x 
Y , у 
г E 
Te T E |; Лл m 
(925%) 224 Ey 5 буу 0 
po a E o7" T^g 0 
X ~ 7 ху 
ak DT а | бат 
2 у Cay, 
Tete 0 
y 
symmetric 
2 2 
Ton 
O 


= 


} 


ее сато malrix serves not only to take Anto 
account the possibility of maneuvering, but of model 


iraccuracies as well. Q is also used to prevent the gains 


lj 





of the filter from approaching zero as more and more data 
is processed, by insuring some uncertainty in the predicted 
state values. (See Sage and Melsa [36], page 415.) 

The observation equations are nonlinear in the states 


and are given by 


Dr 
осо] | tan 209+ vec) (3.188) 
z(k) = = 
Ba) US 
f(k) E ы M У СК) о 
| | р + х,с08(6, 00-000) | 


xj 5 


The doppler equation can be expressed in terms of the state 


variables to give 


ВЕ, uS | 
M u a ee 
x(k) v,(k) + y(k) Vy (k) 


U NE 
E кк даа 
PO (x(k)? * y (2?) 2 


The measurement noises, vg (X) and Ve(X), are assumed to be 


zero-mean and independent with a covariance matrix 


eae | (3.20) 


Inemaenitude ol the angle поі зер із а function of 


the processor used and the signal-to-noise ratio at the 
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sensor. A typical accuracy of * five degrees is common for 
Strong signals and tnis value was used as the standard 
деутас топ Гот most of the simulation runs. 

Тре resolution of the frequency measurements is 
equal to the inverse of a recora iensen оспе сіе 
signal. For a 25-second record this would be .04 Hz and 
this value was used throughout the simulation as the frequency 
E ise standard deviation. This value would represent a one- 
bin uncertainty in the coefficients of a fast Fourier 
transform processor which typically could compute the trans- 
form in less than 100 ms. 

Equation (3.4a) can be used to give the linearized 


obsemvatjon matrix. The result is 

















200) 9е(к) де(к) де (к) 26(x) | 
X ae ду ov. P 
H(k) = 
of(k) 8f(k) 9f(k) f(k) 9f (X) 
OX ду, ду 9v. 91 | 


When the derivatives are taken and evaluated at the 


Predicued states valudsmex'(k) thef result Us 


Ле: 





H(k) = 





1 
y'(k) | 0 
2 2 
x'(k)+y' (k) | 
---- m — - - — - - - + — — - — — - Cir} 
| 
mag? ROT бд ET“ х' (к) 
rule үтер” ЕСТІ терт тұру 
Fo QV [x GO ЕЗ ° ^ FQQOv, rt day 77° 
| 
| 
x! (Kk) чу! (К) | 
EBENEN. > в м... Ш 
tg) О и у" 0001 ғ" 0) y'Qo 
— е ---------т Е Pe a Does A er 7А 
Бу (ум, [xt Ок) y! (953% | Бобом Тк (уау (621/5 
A 
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кү IA y > . can 
| WE) 
Ви 
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The $ matrix, Q matrix, R matrix, and H matrix are 
then used in the Kalman filter equations Е 

Imc c Тег. реп Pos Licen coordinates wilt 
КООШ СОИ О functions of time. Furthermore, a change in 


either heading or speed will result in a change in both 


ya 





velocity components. Thus, the calculation of the state 
excitation matrix, Q, involves all state related terms 

except for those involving Ро. Since a constant-speed, 
constant-heading target is assumed, a different set of states 
can be found which will decouple the heading and speed 
excitation and involve the estimation of only one linearly, 
time-varying quantity. 


Let Ro be the range to the target at the closest 


pa 


point of approach (cpa), es be the distance to cpa (negative 


before — positive after), v, be target speed, 0, be target 


S S 


heading and Po be target rest frequency. The geometry 15 
shown iin Fig. 3.2. Expressed in this coordinate system the 


state equations are 


Қора (581) Ropa? t Е Св Ту >К) 


> 


Хсра(К+1) Хсра(К) Жо e2 r8 Ту 2k) 


v (k+l) = v (k) + (ү, ) и (3.23) 


S 


И 


0.(k+1) 9. (1) + gy Org ) 


\ ж 1 ` 


ПЛ through 65 the random forcing terms. 
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Thessitate transislon matrix is therefore 


ы 1 о 0 
А 1 0 
1 

L 


The angle observation equation is 








— аб с a 
9(x) = 0,(k) - tan [ Е Тен y, (E) уе O 
сра 
Х (к) 
сра 
А.К. (К) 
МОК tan М ера) - 180 + v,(k) X  < 0 
5 X (к) Ө сра 
cpa 
(3.24) 


A 1s an angular rotation term needed to give the 
correct sign for a given geometry. А 15 +1 Гог counter 
clockwise rotation about the sensor and -1 Гог clockwise 
Potation: ` 


The frequency observation = а о Is 








¡OS -e 
f(k) = 22 E o + ve(k). о. 
I)e. ck 
"N = — 
2. Коо туд 
о | а) ] 
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The excitation matrix, Q, for the states is found in 
a similar manner to that for the X-Y coordinate system. А 
heading change effects both a and Хеба while a velocity 
change only effects Хара” The random excitation will again 
be modeled as zero mean, piecewise-constant random heading- 
angle velocity, random change in speed, апа random change in 
rest frequency. With the expressions “or the random forcing 


terms included, the state equations become 


+ = a ° е > 
Ropa 1) Пара? + А Хсра(К) ШЕ T 
А (к+1) = X EO NEL NUES + 12 T? = A-R (k) y: Т 
сра cpa S 2 ма сра o 
па ан = v, (X) t Y Jf! (3.26) 
9. (k+1) = ek) р Yo EST. 
PAUL) = Ше (К) t E T 


The Q matrix can thus be found and is 






cpa 0 cpa^cpa 0 | сра ы 
лак = ЕСТ RSS. m 
(12 ‚ R To: 2! f 2 | —А-В___Т©в; 2 0 
2 cpa E | cpa 8 | 
5 | 5 5 
¡AA ДИЊ NE d 
| 
Q(k) = To.^ | о 0 
5 
m Asse ^m 
| 2 
тор o 
Symmetric | 7 | 
тео, 2 
O 
L_ --! 
(Ве 


Here only the first two rows involve state related terms. 
The nonlinear observation expressions are obtained 


the same way as in the X-Y filter. The resulting matrix is 























де (к) 9е (к) де(к) 96 (к) 36(k) 
MES ox ҰЯ 30. Ж, 
ео 
9f (k) 22469 ar(x) of (kx) 3f (k) 
ЭК ора Дете ТА 30. Ji 
(3.28) 
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-А.Х! (к) Г А-В! _ (k) | 


IP EA |. cpa = | 
R' (1)? o R'Q? | 
| 
| мемен о а 
H(k) = | | 
t 2 1 ' КҮ! ' 2 2 
PER) vk) | Вера (К) Хара | ) реа ул (К) | Нера | 
' 3 | 3i 
BU) vy В! (К) BUC), R'(k)> | 
| 1 
— — -- — —— — тен == — = = ә (3.29) 
2 i | | 
Bos ua e, ; £'(k) | 
F'Ü)v. RU) = AMA 
O р | 
where f'(k) is the predicted frequency measurement and 
R'(k) = [RE + X _ W172 , (3.30) 


cpa cpa 


is the predicted range. 
Phe ФС ан ondaa mabrlees aregegaio. substituted an 


the Kalman filter equations át each Cine polnt. 


B. SINGLE SENSOR INITIALIZATION 

For a deterministic problem, since there are five 
unknowns, a set of five independent measurements or equations 
are necessary. With a sensor which measures signal arrival 
angle and signal frequency, this wotld require measurements 
Irc е петела сев To give Six equations. Because of 


the transcendental functions in the doppler expression, 





these equations cannot be solved in closed form to glve 

che unknowns, and some type of numerical technique for the 
solution of nonlinear equations must be used. The addition 
of the measurement noise complicates the situation further, 
especially for the angle measurement since the senis noise 
may be large. 

To reduce the effect of noise, the difference in the 
angle measurements at different time points should be as 
large as possible. Appendix C shows the probability of 
having a given error in the angle difference measurements 
as a function of the measured angle difference, with the 
measured difference in standard deviations of the noise. 
Figure Cl shows the larger the difference relative to the 
expected value of angle noise, the more accurate will the 
measurement solution be. This may be difficult to obtain 
r three time points in a practical situation, since the 
target may not be held for a long enough time to show a 
large angle change due to fading signals or convergence zone 
propagation. 

There is an alternate method whereby an independent 
measurement can be obtained by looking at the rate of change 
of the doppler shifted frequency with time. The rate of 
change can be approximated with finite difference expressions 
and, because Of range dependence іп the derivative, the range 
can be calculated with measurements at only two different 
times. Furthermore the heading of the target can be found 


if the speed of the target is assumed known. The condition 





15 usually known ‘to sufficient accuracy to give a good 
heading estimate. 


The Doppler equation is 


Е 
О 


Г = шой л 57 : са зи! 
1 +12 cos(0_-8) 
V S 
p 

"nene Seco ppler shifted frequency, Ea is the rest 

requency, V, is the target speed, Ур is the velocity of 
ооо ене па» © is the target heading, and 
6 is the signal angle cf arrival. 


taking the derivative gives 





p V 
de A о Mem Ni? 
an > 
[14 — ccs(6_-8)] 
V S 
p 
which reduces to 
af = => Eu ү ѕіп(6 -6) . 99 (3 33) 
at У 5 S or E | 
O p 
Thé transverse velocity about the Sensor is 
E 
Ко Gu = Vo а 0) 5 au 


with Во the target range, which when substituted into the 


previous equation gives 





2 
dt мм T : СПОЈЕ i 
аъ“ Fo un ' Ro Cae) : 19-22) 


Solving for the range R, yields 


I Du 
RE. =2.%£,_ 1. T (3.36) 
© г СО: (56/205 


Substituting finite differences for the derivatives gives 


DV -Af-At 
R + oo — . (3.37) 
| f (A9) | 


Since Т; x 1 this gives 


V *Af-*At 
а = кезі -—— е (3.38) 
г. (л6) 


в 


Equation (3.33) can be solved for tne heading by writing 


Fo'vp: (Gf/àt) 


in CO A 5 (3.39) 


f^(a0/àt) 


and by substituting finite differences for the derivatives, 


and assuming some value for E. the result is 


V -Af 
@ t D. on у 
sin(0. 0) ~ Г:40-у- : (3.40) 





There are two solutions to this equation since 
sin(9) = sin(180-0). These two solutions correspond to 


the up and down doppler case, that is 


= = СТО. р. ж | 
9, up = 180 + 0 ~ sin pj (3.41a) 
and 
-1 и Аг 
8; pown ~ & + sin “[- DA (3.41b) 


The exact difference equation solutions are presented 
in Appendix A. The derivatives above have been approximated 
at the angle midway between the two data points. Figure 3.3 
shows the geometry for the calculation. Note that the range 
can be found independently of velocity. These four quantities, 
the target range and target heading, the target bearing and 
assumed vælocity are sufficient to initialize the extended 
Kalman filters because all of the initial states can be 
aa Tc ted. Lromsthen, 

The first quantity to be calculated is the initial target 


heading 0 from cheMeguatien (3.4113) or (3.41b). The value 


sI?’ 
of the angle difference,A8, comes from the two angle observa- 
tions which satisfy the critérion of being greater than three 
standard deviations of the measurement noise apart. With 
ЕНЕ БОП ТО СНО assumine ncrmally distributed measurement 
errors, Appendix C shows that the probability tnat the 


fractional error in the angle measurement, 0 „/46, where 0, 


is the measurement error, апа A0 is the measured angle 
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3.3 Calculation of Initial Range, Heading, and 


Rest Frequency 


N 


192 





tz t -t 
A0 «8 - 0 
At=ft-6 
a | 
ы E 
с 4 5 9, 
b. 
ME 4 à, f 
f 
7 
| 2» 
ла > 5 ор 
| V Af 
8. =@ - 160 - ям"! |- —2 
Si AVE 
| | | AQ vs, f 
à и NA NE 
По 77 (AO0Y + 
: 4 
V 
_ S| Р 
КЕ m +2 соз (0. -0) | 
Eq 
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Дъ Тегвпев, 15 less than .5 is equal to .7. This criterion 
was somewhat arbitrary but was found to give satisfactory 
results in most instances. І represents a compromise 
between good initial values and long waiting times to start 
the filter. Note that instead of using — the endpoint 
measurements to calculate the midpoint angle, any other 
measurements or the angle taken in that interval may be used. 
The appropriate weighted average is used to obtain the 
result. For periodic measurements this is just the simple 
average of the measurements. The midpoint angle calculated 
in this manner is referred to as Ó AVE • 

The value of the Prae di ference., Al ye l1Geoptaaned 
by subtracting the first and last frequency measurements 
obtained wnen the angle condition is satisfied. The quantity 
Vo comes from the known propagation velocity, f is taken as 
one of the measured frequencies, and Mei moeche initial 


guess at the target speed. When tre quantity 


RD T) 
LEM (3.12) 


sin(91-8pyp? EC Һ0.1%у 1 


esce ulated eond ites absolute value Xsegreater than one, а 
correction on the velocity guess may be made to bring 
less than one. Geometrically this means 


lsin(e__-8 
D 


1 AVE) | 
that for the quantities measured, the speed of the target 


was not large enough to traverse the angle measured. 
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When this is done OST may be computed using equation 
(3.4la) or (3.41b). The approximate knowledge of the rest 
frequency and the frequency measurements are usually suffi- 
cient to determine whether the target is up or down doppler. 
The range to the target can then be calculated by using 
equation (3.38). 

The initial rest frequency can be calculated by averaging 
the doppler equations, again over the time that A9 was being 


measured. The doppler equation is 


D 
fi W s e a a , (3.43) 
SI 
A = cos (6, 1-04) 
p 
where Г. and Ө; refer to the 27 measurement in the start-up 


interval. FOI is the initial rest frequency to be calculated. 
Solving for the initial rest frequency FOI gives 
M 


Рот = МЕ а VS cos(6.,-6,)] , (3.44) 


and averaging over the initial KJ measurements gives 


AN KJ VsI 


p ds ШІ 4 V сос ш м0 у) а (3.45) 


Wiilch is. tor small angle differences between angle measure: 
ments, approximately 
YsT 


с E. ` > НА 
GE AVE cos (OSI 8 АЕ Ј : СЗШ. 


о Bu 
— ча 





Once the initial range, heading and rest frequency are 
Gotta en ene initial conditions for each filter can be 
found. For the X-Y filter the equations for the initial 


filter states are 


X49 = x = ща cos Өлүк 


X40 yo BS sin 9 Ayp 


Xyg = Ууо = ШЫ біп OST 

Xeq = For (3:47) 
For tne Repa “ера 29433 che initial states are 

Хо“ Вьет 7 A'RgoSin(O8.r-O, vg) 

Хор = Хорат 7 Еос08 (9 1 бдув) 

220 в“ 

О. ы 

X50 E Por ' (37148) 


where А, as before is the rotation parameter. 
The initial states which are calculated by this technique 
= пок шеже зе тасеа State values*ateartime approximately 


midway between the two end measurement times and at an angle 








equal to 0 АЕ The state equations are now used to bring 
the states back to the first measurement fd and 91 and 


filtering begins with this value. In this manner all of the 
angle and frequency measurements that were not used AA. the 
initialization phase can now be processed. These would 
correspond со che time points t+T and t+2T in Figure 3.3: 
One вевъ of this! Initialization technique is to corre- 
late the measurements in the interval with those states 
which are a.direct function of 9 AVE or Куур This violates 
the assumption usually made for Kalman type filters which 
is that all measurement noises are па ааа with the 
Tnitial filter states. 
This effect decreases as the number of points, KISIN 


to “me 


Ф 


the initialization interval increases. This is du 
averaging that was used to calculate the midpoint angle and 
frequency. Consider the covariance between an angle, 0. › in 


the interval, and the average over that interval, Ó AVE? 


which is used for position determination. The covariance is 


cov[6 . ,9 jg d вина. баш (8 Ave дуғо) > (3.49) 


where К, and О АЏЕО are the mean values. Writing out the 


ү те ыар {сус 


соу (0 0 Аз) = E [(0 , -0 Т. 


——— + M Emmen => 





which can be rearranged to get 


cov(8,,0,.m) = EL(8,-6,,) 


еп DE 6, -6 Ө 


Er D i ae 0 ы 
51) 
Thus, the covariance 15 
5.“ 
cov(80, 0, p) = г > (3:527 


with о" the angle noise variance. The same results apply 

to the frequency measurements and PAVE which is used to 

ЁК Л ле йе Tesi ral For: 
The other requirement to initialize the filter е) Ше 

initial value of the covariance matrix. Since all of the 

initial states for each filter were calculated from the same 

set of measurements, the initial filter states will be 

correlated with each Other. This correlation shows up as 

OS meer iow сме" сал ситасесш Initial covariance matrix and 

represents the relationship petween the initial data and 

Die comer llation ol the initial states. Put another ways 

nel ceros terms help describe the region in state space 

UNC Cem ЕТТ с1а sCatesfare most likely to De-as 


specified by the given measurements and the method of 


al starescalculatıion. 
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С. MULTIPLE SENSORS — FREQUENCY ONLY 

1. X-Y Filter 

Zo add additional sensors to the filter, the 

observation equations at the new sensor must be found. 
Thus, in equations (3.18a) and (3.18b), the predicted x and 
y distances to the new sensor were used in place of the 
predicted states x and y. If the new sensor is at XpoVp 
the predicted distances are 


Yor У - Yp (3.539 


The differentiation is then performed and evaluated at 
the predicted state values to give additional rows to the 
na bi 

With am additional sensor ol frequency only a 
catiereny initialization scheme was used. The rest frequency 
and target heading, midpoint angle and approximate range, 
were obtained using the single sensor equation. From the 
doppler equation for the second sensor an average angle 
ЕГОШ nes sensor bo. une target can be approximated. 

Let 0. be the unknown angles to the target from the 
second sensor, measured during the initial measurements on 
Не 2. sensor. Thus 1 goes from 1 to KJ. The frequencies 


Г. are the observed quantities. Thus 
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ри = MENU S, O D (3 51) 
у Т У „те05(0 1-0.) 


р 
KJ frequency measurements will be made from the 
second sensor and it will be assumed that they are made at 
the same time as the measurements on the first sensor. 
Adding the above KJ equations and solving for the 


cosine cosine terms gives 








KJ Ур KI Por 
Y cos (0, 1-9.) ee asl Can = RU (3.55) 
i=] | sI ізі 1 | 
Те+ 
1 KJ 
сов (8 1 болүк) КТ e cos (8.5 0.) 
апа 
ee с 
ONO dat) ta 
Therefore, 
V Е 
a = | ер” ol 
Ө = 9 + сов sail (= - 1) | 5 (3256 
2АУЕ sI Var Колун 


As illustrated in Fig. 3.4, there are two solutions 
pee rename omit rom the sensor vo the target. Mach of these 
can be used with the average angle over the same time 


interval measured from the first sensor. The solution to 
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Fig. 3.4 Initial Range with Second Frequency-Only Sensor 





this triangulation problem which is closest to the single 
sensor range is then used as the initial range for the 
filter. 

The reason the second sensor is used at all in the 
int aue I hZation is to Е thet the measurements of 
frequency of the second sensor will be reasonably close to 
the predicted values that the filter will obtain for its 
first few samples. ТР this is not done, the predicted 
ел e coa Oi that Une linear expansion amy the 
frequency term for the second sensor used for the H matrix 
is invalid, and the residual will be so large that divergence 
Cours: 


ze Кера Хера Filter 


With tne Вс ӨП filter it is more complicated to 


p cpa 
add additional sensors because the states were functions of 


the first sensor only and not of the coordinate system as 
is the case for the X-Y filter. The derivatives in the 
варто linearization must be taken with respect to the 
пре рој ел ве Розе расу. Equations (3.24) and (3.25) for 
the observed frequeney and bearing angle at the sensor are 


general equations if Ro and ша are referenced to that 


pa pa 


sensor. As shown in Figure 3.5, R 


and X for a 
cpas с 


pas 
БЕС Есепсер calmbe written in termsfot the Repa ana ора 


ОТ Пе first sensor. which @re istates! of the filter. “in 
these expressions the angular rotation parameter A, the 


Gietence to the ¡Second sensor, d, the bearing angle to the 











Fig. 3.5 Geometry of 2nd Sensor for R = 
cpa C 


Filter. i 


ра 








secon sensor фр. апа the.target heading, E are all 


significant factos The result is 


n 


+ ә a — 
Repa, Repa А.а»віп(ф ө.) 


Х ера, = авер, - 4 cos(d-8,) Se 


Note that this Calculation could result in a 
negative R ; however, this indicates that the angular 


cpa, 
rotation about the second sensor is different from the 
first sensor. 
The resulting observation equation for the second 


sensor is 


D 
рык = р (3.58) 
E" ИА ene ра -соз(ф-6. (к))) 
SP а 
р Ro К, 
where, 
Rede (Rona (ke) + A*d:sin($-9, (k)))* 
+ (Хе (к) - 4-сов(ф-®„(Кк)))° , (3.59) 
Ата. 


A- (Ropa (K) tA a-sin (4-0 (K))) 





—= = 


б„(к) = 8_(k) - arctan ( 
v - d:eos($-0. (X 
Хера 4“ сов (ф о. ( )) 


(3.60) 
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There КЕ aneacorstional term of —180° 


Ех 
C 


equations are then differentiated with respect to the filter 


5 is less than zero, that is, before cpa. 


in the last equation 


These two 


States to give the new rows to the observation matrix H. 


The resuit, omitting the time! argument, is 


with 
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К Ур 
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ШО Ир 2 
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Ro 


———=— [ sin(9-0,) 
R 


R 


B 


2 


2 
f, УИ ра А: 9: 811(9-0.)) 


„9 сов(ф-8,)) 


2 


ра 


(22615 


= >. AL : 5 . к. % 
v d- coslo О i: desin Y 0 2) 


A A пери о eS ae 


(3.62) 


(3.63) 


(3.64) 


cos (d-0,)+X „„sin(6-0,) 


(> 


— 





== ——— eee 26 
Los и ДІР? | (3.66) 
and 
98 
О ти 
ЖС 2. ЕЦ. bu 
with 
Р 96, У А(Хора”2:с98(%-6,)) (3.68) 
MO AR ET 2 > 
cpa Но 
I k 98, и ACR, ey E 
22 9 ера pre 
99 
T e 0 
о О (3a 0) 
5 
р 98, | à(X, a 608(6-6.)-A-R.,. -sin($-0.)-d) ae 
921 м гг. LS p EE M AT MS 
S Ко 
90 
OL = wee = 0 E?) 
25 д Бы, 
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IV. DIRECT METHOD.OF COVARIANCE CALCULATION 


In Appendix B a method of calculating variances for 
functions of independent random variables was extended to 
include the calculation of covariances as well. This 
technique was used in order to calculate the initial 
covariance matrix for the two types of filters. Тһе 
resulting values Were in some савев an order of magnitude 
larger than one would expect based on the accuracies of 
пе measurements and the geometry of the problem. 

The reason for this is the highly nonlinear nature of 
ШЕСИ ест Ооо лол ува о сре оі ста сопал јон calculation.: 
The computation of the heading angle, for example, consists 
of the arcsine function whose derivative approaches infinity 
as the argument approaches plus or minus one. In reality the 
range of values for the arcsSine function is bounded by plus 
or minus ninety degrees which represents the target being 
at cpa. Thus when one uses the derivative formulation the 
variance or approximate error appears much larger than it 


really is. 


А, INITIAL COVARIANCE MATRIX 
An alternate approach was developed which more accurately 
reflects the error variances of the initial filter conditions. 


The measurements Af, A6, On? and the initial speed guess VT? 


are considered to be Gaussian random variables with known 





Standard беутатс ойе Perea on ets measurement noise and the 
assumed speed accuracy. The changes that occur in the 
calculated initial conditions when each of the measurements 
changes by plus or minus one standard deviation, are 
measures of the extent over which the initial conditions 
will vary and thus are an indication of the accuracy of the 
calculation. These changes can be calculated using the 
boundary conditions imposed by the geometry of the problem 
and are not restricted to a linear region or a smooth 
Mc ion as required їп the derivative method. 

In Fig. 4.1 is shown a one-dimensional example of a 
nonlinear function g(x). The tangent line drawn at x, * is 


the derivative of the function at x,* and its extension by 


k 
one standard deviation shows the magnitude of the error 
computed by the EDUC method. Also shown is the direct 
calculation of the changes in go) when x, * is changed by 
опе standard deviation. For this case, the errors one might 
expect are much larger than the derivative calculation 
indicates. The difference of this up and down direct 

Ser culation. стутаеа ву two, 15 therefore takem as an 
equivalent "standard deviation" for the calculated value. 
This is done for each of the three initial measurements and 


V While no implication is made that this value represents 


sI’ 
лек сгце standard devlletion of thes»partícular initial 
ПО Јо о does represent, for monotonic functions, the 


range of values for a one standard deviation change in the 


measurements. This in turn is a measure of the accuracy of 
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Fig. 4.1 Taylor Series Expanslon Versus Direct Calculation 


И 1 


of the calculation and indeed this is what is ultimately 
needed for the initial covariance matrix. The off-diagonal 
terms in this matrix can also be calculated using the method 
of Appendix B with the derivative times the measurement 
variances replaced by the calculated "standard deviations" 
of the initial conditions in equation (B-20). The sign of 
the covariance terms will be preserved by considering only 
positive changes in the measurements. An example for the 

=X filter illustrates the procedure. From equation 


Ropa epa 
(3.48) the initial range to epa from the sensor is 








1 - А.В "51п(Ө 7-0,0) Е (ШУ) 
which from equations (3.38) and (3.42) gives 
ЛЕТ. ADV, 
el el eee) (4.2) 
NO ет A0-f vor 
Rearranging terms yields 
R = А. LL перо (UE 
cpal ле AG CIV. У | 
which can be expressed as 
у 
Е 5] 2, TR 
Repal = А E oi sin S 9 лур) (4.4) 





Written in this form the boundary condition on the value of 


on is evident since the sine term is never greater than 


опе. 


The equivalent standard deviation in R Gue to a one- 


cpal 


Standard deviation change in A0, for example, is 


(Аб+с,) - (48-0, ,)] A) 


E А 1 


cpal Ropal 


In this case the boundary condition is imposed in the 


Pa culatiensot the sine term in eguation (4.4) above and 





ИУ 
the maximum value for the range 15 > Emus 
Af v. 
(sane re m: ME (4.6) 
sI “AVE Аб-о0 ла? 7 Ver = 


This calculation is done for each of the measurements and 


for v The total equivalent "variance", since the 


sI' 
measurements are independent, is thus 


к 205 ) (и) 


срат 


= о © (16) * Og 2(дғ) + y 


cpal R opal cpal Si 


R 


Similarly, the equation for X equation (3.38), the 


срат” 


distance to or from cpa can be used to calculate its 
equivalent "standard deviation". Since 


E à = l 
x R cos(0.. 0 ) , (4.8) 


cpal O AVE 


T3 





R, can be substituted from equation (3.38) to give 


sin(® m) o coS(6. 1-0, vg) - (1.9) 


Combining the trigonometric terms yields 


cpal ле 2 : 





ln this instance the geometric boundary is imposed on the 





calculation of the angle АЕ and restricts Хара! to be 
А; 
no greater than the maximum range allowed which is 5 м 


The equivalent "standard deviation" can be. calculated 


as was Gone for Ro and the equivalent covariance found by 


pal 


[Е хто ог equation (B17). This results in 


(ле) + 
срат 


соу ГК ] = бр (ле) са 


er А 


срат  срат 


Op (АР) га 


(Af) + Op (v 
сра1 


Х срат срат 51 срат 


(ШОТТ) 


ihe entre initials cevariancesofeestimation error matrix 
Кл реги ed out in this ®manner. 
он сешотизнтка се wnewsdit ferences in thes two error 
estimates, Table DI contains the initial covariance matrix 
for a computer simulation in which the target was near cpa 


when още Diver was ітаттатттей, These values were calculated 


7 N 








using the partial derivative expansion described in Appendix 
B. The run was made using the ве filter. Ти the 


upper left is the variance of R Next on the right is 


cpal” 


the covariance of В. апа А Dal? then the covariance of 


pal 


R and ушт» etc. 


cpal 

The diagonal terms, which represent the initial error 
variances, can be expressed in more convenient units to 
give 


R = = ша 7 X vås)? 
cpal 


var(R ) = со 


cpal 


or с UNE vds? 
срат 


Smadar ly g 14.4 K yds я 


Хара! 
D = 1.68 yds/sec , 
SIL 
To eT. 5 
SEL 
and ср "T (4.12) 
oI : R a 


The accuracy of the initial estímates is typically much 
Detter than these standard deviations indicate. With the 
Same set of data, the direct calculation described previously 
was used to calculate the initial covariance of error matrix 
and the result is presented in Table III. The standard 


deviations for the diagonal terms are now 





216.96 184,20 Е.Г) -17/.07 EO 
184.20 207.28 16.43 -17.83 36.37 
-5.73 -16.43 | 2.85 ем -2.63 

ша 01 -17. 83 1.21 156 -3.16 
се 36.37 -2.64 -3.16 Sale 

TABLE IT. Initial covariance matrix using partial 
derivative expansion. 

78.84 30,3% -0.23 -3. 89 | al 
30.34 39. 29 -7.84 -3.52 126 
20.23 -7.84 2.85 0.60 21.39 
-3.89 23.52 0.60 0.34 -0.69 
ee 1.36 =1.39 -0.69 1.43 


БАШБЕТ L a сегпаглапсс masbLnix using airect 
calculation: 
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Tr = 8.78 K yds Е 
cpal 
бу = 6.27 K yds : 
cpal 
O = 1.68 yds/sec , 
ү 
51 
О 24 33.5? В 
9-І 
апа бр = 1.2 На я (4.13) 
ol 


ue mete fect Of the partial derivative methcd of covariance 
matrix calculation is to deemphasize the initial Conditions 
in favor of the first several measurements. The seccnd 
method applies a filtering weight more in line with the 
eecuracyeot ach of "fhe initial states. 

Not only are the magnitudes of the errors quite different 
but the orientation of the error ellipses which are determined 
by the covariance terms are different as well. This is 
illustrated in Fig. 4.2 for the position variables Ша апа 


pal 
^ срат* поме не во “сотагтаисе maprıcesın Tables IT and III 
a coordinate rotation is applied to give uncorrelated position 
coordinates. Figure 4.2a shows the orientation to be aligned 
with the computed values of Корат апа РЕ. and not dependent 
on the sensor position. Figure 4.2b, for the direct calcula- 


tion, shows the error ellipse oriented with the bearing line 


Brom che sensor to the tanget at the initial position. This 
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Fig.4.2 Error Ellipses for Initial Covariance 
Calculation | 





rotation is believed to occur because of the boundary 
conditions imposed on the calculated errors and represents 

a more precise picture of the uncertainty regions associated 
with the initial state values. 

B. GENERAL COVARIANCE UPDATE AND APPLICATION 

TO KALMAN FILTER EQUATIONS 

One of the problems with Extended Kalman filtering is 
that the linearity assumption used when the nonlinear terms 
are expanded may not be valid, particularly in the early 
stages of filtering when the initial filter states are 
significantly different from the true values. There may 
be MC where boundary conditions impose constraints on the 
range of the nonlinear functions which cannot be handled 
easily using the Extended Kalman approsen, Іп tae final 
analysis the iinear expansion will only be valid when tne 
error estimates accurately approximate the true errors in 
the problem. 

To obtain these accurate error estimates, the method 
developed for the calculation of the initial covariance of 
estimation error matrix described previously, can be appiied. 
In this development one additioanl point becomes clear: 
the partial derivative method (Extended Kalman), requires 
the linearity condition to be valid for a region in state 
Space extending one standard deviation in all dimensions. 
When this condition is not satisfied, the direct change 
method will provide more accurate approximations to the 


mar Ors . 
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Consider a discrete problem with n states and m 


observation variables. 


х(к+1) = £(x(x)) + wí(k) 


z{k) = h(x(k)) + v(k) а са) 
For an Extended Kalman filter the equations have the form 


х(к+1) = Ф(к) х(к) + и(к) 


z(k) = H(k) x(k) + v(k) , | (1.15) 
with 

ue = ті ао (4.16) 
апа 

ah 

Н(К) = 5х Кү | (4.17) 
The predicted state covariance matrix is found from 

О 70: (4.18) 


where P(k) is the current covariance of estimation error 
matrix. To see how the linear expansion affects this 


паше а сена  рте-Гавјог and post-faetor a diagonal matrix 
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of the standard deviations of the states out of the 


covariance matrix. Р(К) is given by 


2 TA: 
с Со о) 5. 
E T2 
cov(x4 x5) oy 2 
P(k) = 2 
2 
cov(x,x,) к EN 


Performing the pre- and post-factoring yields 





| соу(х. Хо) 
o, + ја рту = 
Xq о 
1,2 
С 0 eov(x. x.) 
2 O. O -- 1 
P(k) = Е a X, 
0 | | 
О | cov(x x) 
Xn A 
db diea 
с 
ү 
E 0 
O 
0 52 
бу, 
ог В СС) ~ СК). 
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(4.19 ( 


Era 
SET E. 


(4,20) 


(ШОО) 





L is the diagonal matrix of standard deviations and 
ри пнесори ! aod aszthe matrix of correlation coefficients 
whose elements are 


covixixi)- 


[2], 9" ————À— Е (4.22) 
1) О О « 


те the prediction equation (4.18) is now applied, the 


result is 
Р'(к) = ф(к)-Е(к),Е(к),Е (к). (к) + Q(k) (2.23) 


The matrix product $(k)+*Z(k) is given by 


[ 9f. г, Г 
paa €——— O 
9X4 0% x 
91. О 
O(K)E(K) = | ° 8x, 2 
* е e 
of г 
X In | Ox 
al n 
"jet. | 
of af, af, 
Гани Эх. Ох n ES 
T D. 42 x 
= ТӘ А (Д 24) 
| 9X Хо . | 
у x 9X, an 
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The elements of this matrix product, represent the linear 
expansion of the function f(x(k)), carried over & one-standard- 
Сеута 1 оп interval of each of the states. Thus if the linear 
expansion is not valid over that interval, inaccurate esti- 
mates of the predicted covariance could result. 


However, if each element in the 0(k)2(k) matrix, 


of, (x) | 
e n is replaced by 
x. j 
J 
ER Xps eea Xj tO 0 + 1 9 Xp) - Аты ue i es 
ОРН" Р>"""РУ RR 
> | 
| (4.25) 


then, even though the function is nonlinear in the states, 
Ehemsvalves obtained for use in the prediction equation (4.23), 
will be a better estimate of the actual errors. 

Each of the Extended Kalman filter equations (3.5a) 
through (3.5?) can be written using the direct change 
calculation. Let F(k) be the direct one-standard-deviation 
expansion matrix for the nonlinear state vector function 
Hx Cis and H(k) be the direct expansion matrix for h(x(k)). 


Then the Extended Kalman filter equations are 


Ek) 


Она аа) т а(к-1) 


t 


ЖЕСЕ КЕСЕЛІ (Ic) (4.26) 
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(к)! (кн (к) Гн(к)в! (юн (к) + R(x) JT? (4.27) 


G(k) 


P(k) 9 P'(k) - E' GOR' GORT Qo -EROOR! OORT OQ ROO 177 


ШОО оу у; 


O[E'(k) - GOOBOOJR' GOZ'(O . (4.28) 


L'(k) is the predicted pre-factored standard deviation 
matrix and R'(k) is the predicted correlation coefficient 
matrix. The filter PED SCHON Гот x and the prediction 
equation for x'(k) remain the same. 

Any geometric or physical state boundaries can be applied 
directly to the calculation of equivalent "standard deviation", 
given by equation (4.25), to reduce the range of the function 
and thus the uncertainty. 

While the updated values may not carry with them 
probability implications, as indeed they do not in the 
Extended Kalman approach, they would more accurately 
approximate the errors in the updating calculations due to 


the error covariances of the states.. 


0 
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V. COMPUTER SIMULATION 


To evaluate the two types of filters a simulation pro- 
gram was written wnich computes the true values of sensor 
measurements at each sensor for a specific target track, 
adds independent Gaussian noise values to these measure- 
ments, and supplies the results for input to the two filters. 

For the extended Kalman.filter in the X-Y coordinate 
system, the provision was made to include up to two addi- 
tional frequency-only sensors. The Extended Kalman 
R - Х filter Nes the rrevls. от ЕР ап ааа1тС1опа1 


cpa pa 
angle, frequency sensor, as well as up to two additional 


4.3 ^ 


frequency-only sensors. For the purpose of the simulation 
the rest frequency of Я was chosen to be 500 Hz, 
The speed of the target varied between 6 to 12 knots, and 

a wide variety of headings and geometries were run. These 
values were chosen in an attempt to provide realistic tar- 
get parameters to the filters. When using doppler informa- 
БЕС (һе nigher the frequency, the more the doppler shift 
апа the better will be the frequency measurement for a 
given resolution. At 500 Hz, a difference in Speed of one 


рог реосу 1 пез а ‘sham Gn frequency Of avproximately .1/7 


Ше. 


A. MONTE CARLO SIMULATION 
ПРО ел to compare the per tormancer of the two filters, 


and of each individuzl filter*as various parameters were 





changed, Monte Carlo simulation runs. were made. The calcu- 
lated statistics of these simulation runs then were used to 
contrast filter performance. Each run used input measure- 
ments (chosen for convenience to be equally spaced in time) 
from the simulation program. After each Monte Carlo run, 
the statistics of the filter states at each time point were 
calculated. The sample means, X, y, ende variances, 

Е var(y), and sample covariances aa for the 
position variables were computed. Let the position outputs 
of the filter at time t(k) for a particular run be x, (k) 


and у. (К). The sample statistics for N runs CE 


] Nr 
= хх. (К) (5.15 
` NIS 
rl 1 N A 
у(к) = = Ху, (К) (5.2) 
ее 
^ RERUM CE X И uo 
Rem | ис: = == [2 х.(Кк) - Nx(k) ] (5.3) 
X N- E 
— 5 = о ти [ р x (k)* - М т C524) 
y OA E + i 
A A, 1 N ^ ^ E A 
cov[x(k)y(k)] =r, o,o, = EST D" x; (Oy, GO-Nx (Oy (X) ] 
al 
eu 


nn A A AAA A A A Шә. пеш с сло Зе шш A AA A 


1! 
Deutsch [16] gives the equation for the sample variance 
which can be readily extended for the sample covariance. 





The choice of how many runs to use in a Monte Carlo 
simulation is always a difficult one and must be a compro- 
mise between the computer time for the simulation program 
and the accuracy of the computed statistics. <All the results 
used for filter comparison in this work represent 200 Monte 
Carlo runs. Bucy [11] provides a means for estimating the 
a@euracy Of vie Statistics cale@iated trom Monte Carlo runs 
and his findings are summarized in Appendix D and the 
results given below for the x variable. 

For 200 runs the probability that the тәспі ісе of the 
difference between the specified mean, и, and the sample 
mean will be less than ten percent of one standard deviation 


НЕ ect Трай 15 
DEO xcu EE var(x) PE Sl s (526) 


The probability that the specified standard deviation 
will be within .1 of the sample standard deviation 15 


approximately k95. That le 
Pr 1.96, < с < е) - .95 Ge 


With o the specified standard deviation. 


\ 


В. ROTATED ERROR COORDINATE SYSTEM 
The position variances and covariances calculated by 


the above technique indicates that the errors in the X and 





ios Lion mare” nieniy correlated. Correlatien coefficients 
of .8 or .9 are typical at each time point. 

MPIECthe еггота were normally distributed this would 
indicate that there exists a rotated ccordinate system 
such that in the new system the orthogonal position com- 
ponents are uncorrelated. This is equivalent to taking the 
EXpoment oz the Joint normal probability density function, 
and applying a coordinate transformation which eliminates 
the cross terms. 


The exponent (for zero-mean random variables) is 


“ 





2 | 
A S u 02-8) 


When set equal to a constant, this curve, which is an 
el iipse jets a curve of constant probability. This eliipse 
does not have its major and minor axes aligned witn the 


coordinate system however. By applying the transformation 


x cos 6 + y sin 8 (5.9a) 


P» 
ti 


and 


y сов € - x sin Ө (5.9b) 


се 
| 


08 





with 


Ө = 5 шс ц А (5.10) 


O = 0 
X y 


the ellipse will be aligned with the x', y' axis and the 
resultant random variables will be uncorreiated. The new 


variances in this system are calculated by 





- CMM. 
2 TS | | 
апа 
ре OR 
‚А Se. 246%?) 
gy! 5 " sin 20m (5.11b) 


This method of viewing the sample statistics provides insight 
into the filter performance because it shows the regions of 

high probability and their relationships to the sensor. The 
values of а апа gr ARSS De SiG to as Бае rotarccuter ai 


searlances an tne followinessection. 


C. FILTER PERFORMANCE 

Шрегей тете MOS. aa mni ce mumbo er O: Variables whack 
can be changed, -and their effect on the filter response 
studied. One of the objectives of this research was to pro-- 
duce a workable filter for the passive location and tracking 
problem, and this will be demonstrated through the comparisons 


below. 
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Each figure is a computer generated Calcomp plot of a 
simulated track. Unless otherwise stated, a single sensor 
is positioned at the origin of the coordinate system. The 
line drawn is the true track with the tic marks indicating 
the observation times, and the arrow: indicating target 
heading. The asterisks are the Monte Carlo filter points 
and are the sample average values of position at each time 
point for the 200 Monte Carlo runs. Some of the figures 
also show the orientation of the final sample position 
error ellipse by showing the one standard deviation major 
and minor axes. 

In all of the simulation runs, the X-Y filter gave 
tower final error variances when compared with the - 

Ropa - Хера filter. For this reason most of the simulation 
Poss were rin With the х= filter. 
1. Time Between Samples 

This requirement will be dictated ultimately by the 
computational capability of the computer which would be used 
ГОТ? an actual system. With the resolution used in the simu- 
lation set at .04 Hz, at least a 25-second time record is 
needed for processing by an FFT processor. The time between 
Samples must ре short enough to allow the! filter to correct 
errors, especially in the initial phases. If the time is 
voor tone the result coula be filter divergence or a poor 
estimate of the states. This is ¡shown in Fig. 5.1 and 5.2. 


Тһе first graph is run with 200 seconds between samples and 
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Filter Output and Target Track — 
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в MONE CARLO SIMULATION - 200 RUNS 
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Fig. 5.2 Filter Output and Target Track — 
100 seconds Between Observations 
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second with 100 seconds between samples. А11 subsequent 
simulation runs were made with 100 seconds between samples. 
ае ГТ если пр Сат сомашшапсе Matrix 

The full initial covariance matrix, as described in 
Chapter IV, gives as complete a picture as is possible on 
the probabilistic relationship among the initial states. 
For this reason it provided better filter transient response, 
and resulted in lower covariance of error values at the end 
of a given run. Fig. 5.3 is the response of the X-Y filter 
when only the diagonal terms of the initial covariance were 
used - a common practice in implementing filters. Fig. 5.4 
ғә che Х-1 ff lteér response for the identical set of measure- 
ment data with the full initial covariance implemented. 
ndi A Ela and 5.6 and Fig. 5.9 and 5.10 
show similar behavior. The final rotated error ellipses are 
shown on the plots. In each case the variances were less 
with the full Matrix than with only diagonal terms. Figures 
Bell@and 5.12 and Fig. 5.13 and 5.14 show similan behavior 
pa 7 Хора iller: 
rO Ри сео а A umbe Ще Sensors 

Figure 5.15 shows the X-Y filter response for a 


for the Ro 


single sensor at the origin. Figure 5.16 1s a diagram of 
Ко е ке пета regions Төр this Latter when the rotated error 
coordinate technique is used. Note how the rotated system 
tends to align itself with the bearing angle to the filter 


point. The rotated error ellipses of the учесу сотроп е5 
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Fig. 5.4 X-Y Filter and Target Track — 
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Fig. 5,9 Х-Ү Filter and Target Track — 
Diagonal-Only Initial Covariance Matrix 
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(not shown) tend to show a similar although not as striking 
alignment with the E те“ 

in Fig. 5.17 an additional frequency-only sensor was 
added at the position indicated and as expected, improved 
the averaged filter performance. The rotated variance plot 
is shown in Fig. 5.18 and the presence of the second sensor 
has reduced the variance by a significant amount in the 
Radiat directions This result will, of course, depend on 
the sensors and target geometry. For use with this type of 
filter, an additional sensor will enhance performance when 
it sees a definite change in the frequency it is measuring. 
If the sensor were put near the predicted м for example, 
very little change in the doppler shifted freoueney would 
take place (except when the terget went thru CPA) and the 
changes which the sensor would measure would be mostly due 
to the frequency noise. 

4. Comparison of the X-Y and ра T Filter 

As mentioned previously the X-Y filter performed 


better in all*simulation runs tnan did the Ro - Xa 


pa pa 
ICC тем момгсе Сат тонстасиейцнетенетосет со Lhe true 
track and the final rotated error variances were lower in 
all cases. Figures 5.19 through 5.24 show a comparison of 
the two filters for three particular target tracks. In 
each case the same simulated input measurements were pro- 
vided to the X-Y and Repa = Хора Ше гето тре ате ста 


nucum uyssol positions. velocity and rest. [frequency were 
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Fig. 5.17 X-Y Filter and Target Track -- Two Sensors; 
Angle - Frequency (0,0); Frequency (2000,-2000) 
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calculated as starting values for each filter (expressed 
in the appropriate states). 
One possible explanation of the poorer performance 


of the Ra = X, filter lies in the calculation of the 


pa pa 
position of the target after an observation has occurred. 


For the X-Y filter, of course, all that is needed are the 


States X and Y. For the Ro - X, filter the bearing angle 


pa 
to the target must be found from 


pa 


^ ^ А.В (к) ^ 
ЧО нии “Оол (Ра > fe ОООО ees) 
S X (к) сра 
сра 
ог 
~ A m А.В, А СК) ре 
34) a Mo С хе 0 
5 X (к cpa 
E (5.1209 
and the range from 
1 
IEC a В (5.13) 


Паетшов сола 5 calculate from the range and bearing. 
Note that coupling between the position and velocity compon- 
ents has occurred because the heading angle appears in the 
bearine wamelemcalculation ws. Thussche position calculation 


is dependent on three estimated state values instead of the 
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Fig. 5.23 X-Y Filter and Target Track 


116 





т.059 


| HONTE CARLO SIMULATION - 200 RUNS 
КЕ б. = -709 
s v, = 8.0 KTS 
Н. Vey = 6.0 KTS 
А a 
=.» An d - ; 2.43 ар 5. 54 
<. 25 а 


да еи im Ey and Target Track 


JT | 





two required for the X-Y filter. А large noise component 
in the frequency measurement, for example, which results 
ina significant change in the heading, wiil thus effect 


the position by rotating the Ке Х a R triangle in 


ра’ ср 
неми Joc. 
ШІ ПЫ 665569415 

Poirot [34] in his dissertation studied the residuals 
of an estimation process in order to determine whether biases 
were present in the procedure. For on-line digital filtering 
the use of residuals, the difference pecan the predicted 
Шы шее еи aid tne actual measurement, might be used for 
a Similar goal. In the simulation program, a counter was 
established to indicate when the residuals: were greater 
than two standard deviations for the angle and four standard 
Geviations for the frequency noise. The frequency is less 
sensitive to position errors. These levels were somewhat 
Ра Dut for a poor track on a particular run, che 
counter values were larger when compared to а good стас 
The effect needs further study but indications are that 
mais could be used to reinitialize the filter and revroce'’ss 


the data with the new starting values when the residuals 


ӘШӘЛШ ШОО performance. 
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VI. CONCLUSIONS 


А. PASSIVE TRACKING AND NONLINEAR FILTERING 

The idea of localization and tracking with a single 
passive sensor has been shown to be feasible. The method 
of calculating the initial position, heading, and doppler- 
rest-frequency provides the key that enabled the extended 
Kalman filter to perform satisfactorily. With only limited 
a-priori knowledge, the filter was initialized by waiting 
for a sufficient change in the observations to calculate 
ПОО а со а нон бе state vector was then brought 
back to the first observation time to begin filtering. 
This technique enabieá the filter to start with good enough 
initial conditions so that lock-on and track would result. 

The role of the initial covariance matrix was shown to 
play an important part in the transient response of the 
filter and resulted in overall better performance as measured 
by sample means and sample variances in Monte Carlo simula- 
tions. That is, the average mean-square error at the enc 
ов а given filter run was less when the full initial co- 
variance matrix was used than if only the diagonal terms 
MATAS VEL 

The#caleulation of the’covarlance of error matrix for 
ЕРЕ lines» problem using a simple. Taylor Series expan- 
с Лони масш Пома bo nor adequately reflect the actual &rrors 


that can be expected due to errors in the measured quantities. 
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Mts 15 due to the linear expansion not being valid over 

a variation of one standard deviation in tnese quantities. 

A method was Geveloped which more accurately estimates the 
errors by a direct calculation of the changes that occur 

ШОШ пе пот теат function duel to a one-standard-—devilation 
change in the input states or measurements. In this way, 
any physical or geometric boundary conditions cen be imposed 
directly. In an extension of this technique, it is believed 
that the updating of the covariance matrices used in the 
linear Kalman filter can be accomplished the same way and 
the result will be a more accurate weighting of the measure- 
ncm Ste predlicted filter values. 

Even with a relatively simple geometric problem, the 
ОШО се Or MA lter states can play an important role in filter 
Dperiormance. The choice of thefbest system will ‘betafiected 
by the measurement variables, the coordinate system chosen 
Рог the Gynamic model, and how any filter excitation Ys 
mode le al 

An additional sensor measuring frequency- only was found 
mer eaivessteonificantly lower position “error variances than 
a single angle and frequency sensor. In general it was 
found that the sensor should view as large a change in the 
measurement as possible to minimize the effect of measurement 
MOUS Cs 

ШО с=с аз об he filteri that 13, the ditterence 
Between tne actual observation and the predicted observa- 


tion, were found to be a good. qualitative indication of the 





pIS—XrLEDertornomncewof the 111 сет for a particular run. 
eV a естп те calculated Берг covartance oflerror 
matrix will not show when divergence is occurring. Some 
criteria using the filter meee cuales relative to the measure- 
ment error can be helpful in this regard. A corrective 
СЕ miente to reinitialize the filter and start 
processing the observations over again. "The residuals may 
also be used as an indication of a maneuvering target, 


in which case the assumed state model is no longer valid. 


В. SUGGESTIONS FOR FURTHER STUDY 

The" most obvious extension of this work would be to 
test the filtering algorithm on real data. The capability 
of obtaining tne required frequency resolution has been 
demostrated [46]. The extreme limits on the angle noise 
have to be defined so that meaningful measures of the 
angle позе can be used in the filter. 

МАСА Е оо Git m@uwmaadlCulatz@a or The Covariance 
matrices for nonlinear problems represents a different 
approach in applying the linear Kalman filter equations, 
and this technique should be evaluated against the normal 
PztendenzRkalmanzrormulatTien. 

Durchenstudvzoerstchescheice of’filter states and their 
comparison with each other, is another area of interest. 
The same prcblem considered here could be formulated in an 
В - 6 coordinate system for example. In this system both 
the state and observation equations would be nonlinear 


however. 


Las 





Án additional area of study might include the effects 
of a maneuvering target on filter performance, and how the 
choice of filter states and their coordinate system are 


involved. 


иа 
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APPENDIX A 
Development of Initial Conditions for Single 
pensor — The Exact Difference Equations 
For the two dimensional constant-speed and constant- 
heading target, there are five unknowns which completely 
determine the solution.  Expressed in an X-Y coordinate 
system these are position (x,y), the velocity В and 


the rest frequency of the emitter, F The only a, priori 


on 
knowledge available in the general search problem is the 
approximate target speed and an approximate value for К. 
The measurements consist of a noisy angle of arrival anc 
a noisy doppier-shifted frequency. Using the change in 
angle and the change in frequency, an expression for target 
range and target heading een be found which is not sensitive 
to knowledge of tne rest frequency. The accuracy of the 
range and heading calculation can be controlled by waiting 
until the changes in angle and frequency are large with 
respect to the standard deviations of the noises involvec. 
An expression was derived from recognizing that the rate of 
change of the doppler-shifted frequency is range dependent. 
Given range and speed, the heading is then found from the 
angular velocity expression. 

From Fig. A-1, at tine t, an angle 9- , and frequency T4 
are measured and at time ttkAT, 6 апа Г are measured. The 


doppler equations are 


J.23 










target speed — a 


target course — O. 


DR. 


Sensor & 


Fig. A-1 Geometry of Sensor 
and Target. 
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1 


IA * | (А-1) 
| 4 тк мо 
and 
12 i M уе : 
1 + EB ы. Е (А-2) 


where Fo is the rest frequency and Vo is the velocity of 
росрававтоп ог the*signal. Time differences in the arrival 
of the signal have been neglected since they are small 
compared with the time scale of changes in the frequency 


and angle. Subtracting А-1 from A-2 














A 1 1 
A4 m o ee LP E diss I ] 
1 + A cos (0.705) l + == cos (9-0, ) 
p р 
E Dar | cos(8.-0.) - cos(8,-0,) 
Ур Ye Ще 
Eli с сов (ожа е cos(0,-04)] 
p p 
о лав 7-900. t0: 1:00 
PV. 2 5 тр DURS ПРЕ 
Үр A 8 
Let A0 = 05704 and 
_ 91 + 02 +6. + + 017 
AVE КТ > 
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where ше, i = 1-К4, are the angle measurements taken at 


equal time increments between t and ttAT. Refer also to 


Бар. Dc 
Then 
y Qf * 9 
= Eu e . 1 e Я ло 
АГ y, Po 2 sin(®, Өлүк) sin s 


Solving for sin(8,-0 m) yields 


| Af-F_-v 
sin(O -80 O 
9, п. (A-3) 
Р.Р: у 
2 1 2 S 


AVE? = ш 
2 sin 


There are two solutions to this equation 


9 -6 = sin? [ - SL ] 
s "AVE ла 5 
2 sin 2 Ma o vo 
and 
в о 
O um == =l 4? қ қ PE == 
180 = (ЈА лур) sin [ ] 
2a san Bod i oe. 


The first solution is the down-doppler or receding 
target and the second solution is the up-doppier or 


approaching target. Thus 


= a: = ae Г ое e 
веруј 7 ддџе 7 538 | ] (А-В) 


and 





-1 s 
- o - а i = 
бпр MOOS F 9 аур SIN E ШШЕ = 
2 sin > ГуРу. 
The inverse sine is taken in the interval ЫА 5] ‚ From 


the approximate knowledge of F the up or down-doppler case 


can usually be determined. For small angles and since 








Е 
er the expressions simplify to 
М.Т б 
2 2 
_1 7 | 
берин 7 Paya * Sin авто E 
and 
2 DEON 
= о 4” m AP = 
ieee ayes ein I ша 


Using the Law of Sines on the two triangles in Fig. А-1 gives 


ш ам ји _. 

sin = sin(0.-05) 
and 

Есина еп. Е 

sin $2  sin(e,-9,) 


Mow aber beUedyestaneo tPaveled in time AT and thus 


Ten 





ақ ЖЕСЕ m o pe 
5 2 SEO NE и 
s 2 om ac 
= Y + 1 = 
Ж $11 (9. 0.) sin(6. 97) 


| 2 sinl ð -Tes Шыт 0.794 


: ле 
a лв 2 51 (0 -бдур)" СОБ за 
[cos A9 - cos 2 (0,70, yg) J 


Solving for R and substituting for cos 2(0.-0,/4) 





си. 1 2 .. ' 1 A0 A0 = T 1 
1 2 sin (0 Өдүр) › and sin => cos уз 
result is 

а 
Ф еи Е 2m (6.78, vp) + сов ле -1 
sin A80 2 sin(8.-8,yp) 
^ Vg: AT-Ssin(0.-0, v.) боз МУ | 
sin A80 


w 
2 іп (0,.-Өлув) 


Substituting for 51п(0.-9 ур) from equation (A-3) 








2 


le 


Af: ‚ЕР «AT V. ls 9: 
R = LP. t (cos 4021): 2:sin* BSA) °) 
2 sin == -sin A0'f.'f O Dp 
2 15005. 
Af-v_.-F_-aT pe eel 
2 So ЛЫ 
=ош A 2 —[1 - (l -cos A0) осор: ) ^H 
2 Sin TY Sin A0* f. f O 





ПО typical values of A@, Af, Vea Vp? fi» Го апа i the 


magnitude of the second term is small compared to one and 


can be neglected. For example with 





КО = 15° 

АГ = .2 Hz 
В Е - 

По 4 

F- = 500 Hz 


w= 5.63 yds/sec 


IW m Пе 

Бр о! _ 2 M 

(1 сов A8) (тр = 
DD 





ОСОБИ 


Now using the approximations used previously for the 
heading calculation and neglecting the term above 
A NAME 
A a” "= 
2 
(л0)< Г. 
The error variance of the Range and heading calculated 
above can be determined from the equations іп Appendix B 


and a knowledge of the variance of AT,AS, and Wet Let 


Мат (АР) + де 

Bee > 
var(A@) = 204 

KY 2 
varív,,) E Oy 
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Then from equation (B-1) 








IR 42 2 ОБА 2 2 
var(R) x A6) РОВ r CSAT? 20 ғ 
2 2 
С С 
= рег 8 - + 2 £ 
ЛӨ Af 
90 98 90 
S 2 2 5,2 2 2002 2 S MA 
var(0 ) s (X———)*c + ( 56 + (ед) с ы (---) 6. 
5 9BrvE Ө дут 046 А8 ЗАТ Af 9У Та 
This is shown in Appendix B to be 
2 2 2 2 E 
б R б С бү, 
5 KJ X 2 NI A0? x 2 
‘срат S 
For ЛӨ = 15° oe = (59)4 
MEM о = or) 
v, = 5.63 yds/sec ca 2 
5 у = (1.68 yds/sec) 


5 


var(R) = R(G + .02) = он? = (3600 уд)“ 
В = 3830 уаз 
ве 0036 +. 216[.02 + 28.04] 


| 


(1502 


2250 


es 





APPENDIX B 


Covariance Expressions for Single Buoy 
Tia. God оптовые — -Х А Filter 


pa ор 

The@aini tial cendititons for the filter re®calculated 
from the three measurements described in the text: Af, the 
frequency difference; A0, the angle difference; and Мет, the 
assumed target velocity. It is assumed that Af and A0 are 
independent, normally distributed random variables whose 
means are unknown and whose variances are obtained from 
the measurement accuracy. The variance of Mer is based on 
the a priori knowledge of target speed and it will be 
assumed to be (3 knots)*. If each frequency measurement has 
a variance р then the difference of two independent 
mea stiiement sanas variance до. Likewise the variance of 
the angle difference measurement is Ро. The uncertainty 
due to the frequency measurement itself will be neglected 
Б Шетте 15 smallfeompared to other errors, (o e у >. 
The angle 9 is the mean of the angle measurements taken 


AVE 
before the filter is initialized 





1 KJ 
Ол = 5 г olt.) (B-1) 
=й 
such that 
0 (ty 5) - 0(1) > 3'0g (B--2) 
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The initial filter conditions are derived from the 


following expressions given in Appendix A. 


р 


A-Af^.NT-vy 
R = u (B-3) 
= .ғ. 
SI 
2 
ЛП. Е . 
E Af-AT+v 2 ай AT Ур 2.1/2 
cu = E) - un cH тј 1 (В-4) 
ЛӨ f, AOS "Y Го 
-1 АГ у 
Een = Өлүк SO (— ш ) - 0 
лт ен | COG (В-6) 
2 | 
Por = Е аур = сов(0 --Өлур)1 : (B-7) 


Шея ласта сочатлапсе Matrix consists of ali second 
order expectations over the probability space of the four 
random quantities discussed earlier. The effect of other 
unknowns 2 f) are assumed small compared with the above. 

Papoulis [32] gives an approximate expression for the 
песе елси гаталпеєев от an smooth function of two random 
variables. Below an extension is made for n-random variables 
and an expression for the covariance between two functions of 
these variables 1s found. Since “i, AG, and v, are 
considered independent random variables, and thus their 
covariances are zero, the expressions can be simplified. 


Also since 6 а зе сим Of independent random variables 


AVIs 


lc 





and A0 is the difference of two of them, 9 and A9 


AVE 
are independent. 

Пре УЕ Сов от o imdepenoeuct random ver Peblesgand 
g(x) and h(x) be two scalar valued functions of the vector 
x. It is assumed that g(x) and h(x) are sufficiently smooth 
аео Нее region orf Tite’ mean ота, Xo» that a Taylor 
expansion of g(x) and h(x) about x. can be made Ert 15 m 
ине пас the probability density functions for x is 
symmetrical about the mean and sufficiently compact so that 


central moments higher than the fourth do not need to be 


considered. If g(x) is expanded about its mean, then 




















СО = 6 (х0 + аа: ( ESE к т 
вх) = EG) * 3o Geox) + у (0) ag ee 
(В-8) 
with derivatives evaluated at x=“ x_. The vector a is 
defined by 
98 95 95 
(98) * Е | E 9 са э . . • ҚС ы ] ә (B-9) 
9х 9X4 9%, 9X, 
32 
the matrix ди | ірі гітуел бу 
DN. 
l J 
2 2 
9 8 98 : E 
3x, * 9X49X, X4, Ox 
2 
2 A o E ОЕ : : 
дер = ах аха Бе 
И IX ðX] ти, 22 ( ) 
дә | 
5 | 
ад көт Se 
Хх. ӘХ 2 


ы й E e 
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Taking the expectation of the above expression and 
remembering that the density of x is symmetrical and that 
covariances of the form El (xs =X; о) 5 E m. O because of 


the independence condition, the following result is obtained. 
= 28 Он = Ro 
E(g(x)] = g(x,) * 5 С (EH) 


with 


pce Е 


2 | 
x, Е (В-12) 


Substituting [g(x)]^ for g(x) above 





ЕГе(х) 1 = в(х)2 + 2 Та ЗЕ. + (86)ёус 2 (в-13) 
k ох k Ху 
С 
The variance 
E 2 2 
var[g(x)] = Ев(х) 1 - ELg(x)] 
Tae Te o E 
zx EX, 5 2 эх х 
X k k 
k 
2 3 g іс: 9“ А. 
E E ТЫ 5 0 а È a 
ща m 22 Xy d 9x, Х 


| 





E 


The ‘last term is neglected since it involves fourth order 


terms. Thus 


var[g(x)] > 2 (55—)° с ё = 


г 
k k к к 


Similarly the product g(x)*h(x) is expanded about x, 





~ = Jh == T 
Henna e(z (х) + 5 Е (хах) Er 
е де•ћ ті 
The matrix [5 7] has terms ofthe Borm 
150 
О КО л к 85, 36 
3) 9X, X, 9X. OX, 9х. 9х; 
Again using the independence condition 
E[g(x)h(3)] « gx, h(x,) + = 5 [6 B+ ek 
eo оо об: 


The covariance 15 


соу[&(х)һ(х)] = ELe(x)h(x)] - Elg(x) JE(h(x)] 








(—=— о 


= i oh 
x fe Geh Gu) 5 : [Б 
x 
k 
; 2 
1 oh 
Е ” = T + 
| eG) Gs) + = ru 
k ЭХ. 
С 
i (x CUM E- O SIT 
k 9x,” ^k 2 


др; 
ox 


k 


X 





ye 


98*h 


(B-15) 
to give 
1(х-хо) (BON 
Әср 
Nox x. ^ (B-17) 
a) 
2 
A бо 
X A m 
ke с 3x, 
(B-18) 
3*g | 
De - 
ox: 
K 
+ 
(B-19) 





ЕРЕ РА Ре тоцт огаег бегиб the resulting covariance 


15 
соуГеСудћ(х) = 5 (86—).(80—) а, © 
k k k k 
~ ћ z 
LE) (ELE ш (B- 20) 
k OX, Xy 9X, Xy 


Tne above expressions for variance and covariance were used 


to establish the initial covariance matrix. Let 


Xi = Af with variance 2 2s 5 
-- ак. • 2 
Xo = A6 Valse Vad emcee сө 5 
X, Eye with estimated accuracy given by a 
variance term c 5 5 
Sau 
and X, = 0 with variance c : 
4 і 6 


Each of the partial derivatives were evaluated at tne 
measured value of Af and A0 and the assumed value of Мт 


The resulting terms are listed below for the И рай ера 


filter: 
Е 2 = 2 O 2 
ЖІ er (B-21) 


cpal APS лө? Ver 





var[R nar- = В 




















-2 2 CN 
var[X par] a ЕО a — за 
2 О 2 
2 NO д ҮІ 
ЗЕ eX opal ) 2 ПораТ MS J (B-22) 
ло V 
SI 
var(v r) ој Е (B-23) 
sl 
2 2 2 2 O 2 
О R О O V 
or) a eee Pate om, AS 4 st 7 (В-24) 
= К X 2 hie лө“ у 
cpal 51 
2 2 2 
pU VE es H O O 
во ера Af ЛӨ 
X 2 бу у 
(£pal, - 1) —L 7 (B-25) 
R opal VSI 
where 
i Кари 
Е == Tr) , 
AVE KA ігі 
and 
E a 2 
e a epal 
The covariance terms are 
2 2 
А.В CN О 
А ‚рат ; 2 AÍ ло 
cov[R ER | + pee [X (2 - ar. ge 
cpal “cpal X opal cpal Af лө? 
2 2 O 2 
" O V. | 
И Eo е у, (B- 26) 
cpal Af Zr VSI 


ша > E I 





with A, the angular rotation parameter. 

















O 2 
V 
= A. sI 
en aa 5 (в=2{) 
YsT 
2 2 Е 2 a 2 
ши | срат Af ^0 sI 
s (2 вы 123 -т в? u (В-28) 
2 2 
i N ~ O O 
соуГЕ ыа ч = SI CO al [R 1202 AT o. 3 де 
Вон epal Ets Af ла? 
O 2 О 2 
№ У 
T 2 sI | 
ы = - 
en E ) ерат 2 (B-29) 
sI V 
sl 
2 С 2 
МН V 
= sl сра: 51 
eo = — € ÀÀ— . em ne Tannen — 
E VX al’ st? — Eo (B-30) 
sl 
2 
2 2 С 
А.В O V 
COV] X = с ра 2 A AB 51 See 
[ aoc md X 5 И ри we ш Nee | Du 
cpal Eon 
2 2 
O O 
2 Да ^0 
E о ^ ! (B-31) 
cpal NI МЕ 
2 2 
lo „Г Зи К 
Rangel-X T А АГ AQ 
O 2 2 O 2 
Ми ee ce A E 
R epal Af "T e 
"SI un 


(B-32) 
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у BR V 
с T p 
Ten E E стад > (B-33) 
cpal Ver 
2 O 2 
у К у 
Г 51 АМЕ 2 a ee 
cov[vc Fol ји V R -X (X opal берат ) M 2 (B-34) 
P O cna ET 
с 2 
eR B е 
соо 2] = АЕ ое "ave “sI DER CET 
sI ol RN 2 epal у 2 
Pe DO S 
S 2 = 2 D 2 
А I - 
ar tr er з? (B--35) 
P Af ле Y 
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APPENDIX C 


The Probabilities Associated with Meesurement 
Errors for Different Values of Angle Differences 


Assume that the angle measurements are independent samples 
consisting of the true angle plus additive white Gaussian 


посе iS 


94 7 8 ER: 


tie probability density function of the 42,5 is zero-mean 


with variance к: Thus each Ө; is normally distributed 


Ф е 2 
with mean oa and variance бе 


of the difíerence between ө; апа ЈЕ A0, 15 егетоше 


The probability distribu Ton 


normally distributed with mean 9107 910 and variance 20,7. 


The measurement error 


Ж (0: 704) - (0477850? 


ШСБ А еби. погма Пу distributed with zero mean and 


variance 20,2. From the probability tables 


о < asd, } = Р 


Е 


а 


with Ios pne probability assoclated with a normal random 
variable being within standard deviations of the mean. 


Substituting the angle variance gives 
277412 = P. 
e a / бр) Е я 
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If the angle difference 5% is greater than 3 , then 
EE AG 
ЗАО se 42 37 > D 


This means that for example 


^ 


De ‚5 46} = Р 


| 
Е | а 


where 





or E E O 


Rel probab113ty that a normal random variable is within 1.06 
Standard deviatlons of its mean is 0.71. Thus Гог АӨ > 30 o 
the probability that 0/88 < .5 is greater than 0.71. 


Figure C1 shows a general graph of 


m 
ave ^O = 

SEIS = b } A 

with АӨ = boy» and а! = Ld 


as a parameter. 


TAQ 





Probability that Error |9. | < a'la 8) 





же 
Va] 
PE 274 


/ Дн 
y 


pacc m cmo 
~» в — 


// 


D. 
E у == AS 
| 2 5 4 5 6 


Angle Difference — in Sid. Dev. b 
Fig. C-1. Probability of Angle Difference Error 


for Different Values of Angle 
Difference. 
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APPENDIX D 


The Confidenee Interval for Statistics 
Calculated using 200 Monte Carlo Runs 


Bucy [11] gives an approximate method for estimating the 
accuracy of computed statistics based on the assumption that 
the samples are Prom a normally distributed random variable. 


The sample mean is given by equation (5.1 ) and is 


(D-1) 


54 | 
| 
>4|-- 
ПЕ 4 
Ба 


where N is the number of Monte Carlo runs. The sample 
variance 15 
E 


(x; - X (D-2) 


NO 
Ke! 
Ез 


n N- 1 1 


2 bemthertrue mean amd уатізгес о? сле distribution 


Ес шапа с 
Па) (Ше Өапріев are from, апа thefdatantities (пас "ге ог 


interest. The sample mean and variance are unbiased, that iS 


D(x) -Eu and (D-3) 


2 аи г 2 
B(S, ie ща (aus 


The variance of the mean estimate 1S given by 


750 2 Я 
2 Ех, ) - uM £e 


о (2-5) 


var(x) = gs 


1113 





Тһе variance of the variance estimate is given by 


Ц D 
B(x. ) = (6^) 
ER 2 D 
var(s 2) = 18,2 o2] EL) 


ene oi1stribution of the X,'8 are approximately normal, 
then 


! 


ЕЮ e (= 0 


1 


and thus 


2.2 
var(S, ^) = 0с AC = 2 Lo : (D-8) 


From the variances for x and S ата реза а су сао еске ве 


NEN 


: е em 2 > ~ 2 
confidence region Гог х апа 5 can be round. Let N be 205 


Monte Cario runs. Then 


pes cx |. 5 1) = А = | <а с: } (D-9) 


5 
where g— = po ata апа а 15 спе number of standard 


SED S 


deviations in .1 She Dna с 





ST 
v B SIVE ccu cg ON 
ОР 


а = .1 [200 = 1.11) 


LAH 





The probability that a normal random variable is within 


1.414 y of its mean is „ВН, which means 
| ел засы (D-10) 


For the variance 


1 2 2 = о 
еы? 5,2) к „95. (D-11) 
From equation (D-8) 
зо ES = 2 = с 
5 
п 


Substituting this into equation (D-11) and writing out the 


absolute value inequality gives 





EMT 2 2 ee ee 
р1- 202.52 «82-62 < 208 02} = .95 , 
[N JN 
Or 
ща ! s 2 
Р { 2 <io <x Ее. 05 


With N = 200 this gives 


A spe < g^ < 1.39 с Ша 


OT 


Pp, 1.86 SS 1.18} = ‚95 


їй Б 


mn 





LIST Ole SYSTEM "ETBRARY PDUBROUTINES USED 


GMPRO 
ATAN 
MINV 
ARSIN 
GAUSS 
PLOTP 
ARCOS 
SIN, COS, TAN 
GMTRA 


SQRT 


Matrix Product 

Arce Tangent 

Matrix Inverse 

Arc Sine 

Normal Random Number Generator 
Printer Plotting Package 

Arc Cosine 

Trig Bune tons 

Matrix Transpose 


Square Root 
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LIST OF PROGRAM VARIABLES COMMON TO ROTH FILTERS 


EX exact range at time T(J) from sensor I 

| to target 

ID); exact doppler shifted frequency and rate 

ROOT CE Т) of change of frequency at time T(J) at 
sensor I 

БЕСТЕ exact bearing angle to Фавеею at time T(J) 
from sensor 1 

ЕТ. ор, radial velocity and time rate of change 

RDA TEL. TJ) of radial velocity at time T(J) at sensor I 

Mot), Yo (J) exact X,Y position of target at time T(J) 

pat). Y( 1) X,Y position of sensor I 

T(J) time of observation 

pom YS initial ЖҮ (0810100 of target 

VSS exact target velocity in knots 

yS VSS in yds/sec 

THTA exact target heading in degrees 

THTAS Ши Тай тааап 

ОУХ exact target X velocity 

OVY exact target Y velocity 

FREQ exact rest frequency 

VMED, VP velocity of propagation 

SA, AMA | standard deviation and mean for normal 


angle noise generator 


SF, AMF standard deviation and mean for normal 
frequency generator 


ТАЕ Starting nunbers tor molde Benerator 
AVEVA, VARVA computed mean and varlance of random angle 
noise 
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NSPOT 


2499 
NUM 
МТ 


Z(J,N) 


AVSK 
AVS 


FO 


KJ 


ADELTH 


ADELT 


АТНАТЕ 


ADELF 


ATHETH 


FAVE 


ASINE 


ARHALF 


PP 


ие о а1 тепе tracks Бо пе run 
number of runs in Monte Carlo Clon 
number of time points 

time interval between data points 


number of observation variables 


noisy observations at time T(J), Index N 


determines whether it is an angle or 
frequency and will depend on number and 
type of sensors 

initial guess at target velocity in knots 
AVSK in yds/sec 

initial guess at rest frequency - used only 
to determine whether target is up or down 
doppler 

counter which indicates how many time cuts 
were needed to satisfy angle difference 
criterion 


angle difference which satisfies criterion, 
ед АПГЕГІН ЖОСА 


time difference Corresponding tOo angle 
difference 


time at ' which initial filter states’ are 
round 


SneaueneyadQ\l!i feremeeweorrespondine Lo 
angle difference 


average of all angle measurements during 
т лаш Sacaron TOBY аш оп 


average of frequency measurements during 
gngjtlaldseogdliticnwmgculatuqzon 


Arc sine of angle difference between the 
the target heading and target bearing 


calculated initial target range 


one standard deviation of ADELF, equals 2 SF 
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AA 


VV 


AVAV 


HEAD (ADELF, 
MEL TE. AVS, 


ATHETH) 
HUF 
HDF 
HVA 
HDA 
HUV 
HDV 


NO 


COVK 


соу 
РНТ 
РНТТВ 
SIGTH 
SIGVS 
SIGFO 
F(N) 
QEXIT 
HCB 
HOBTR 
GAIN 
GAINTR 


24015 


опе ыа ато dewlation OLBADELTH, equals e 
ЗА 


guess at accuracy of AVSK, equals 3 KTS 


one standard deviation of ATHETH, equals 
SA/ Kd 


function subprogram to calculate target 
heading from initial measurements 

target heading when ADELF is increased by FF 
target heading wnen ADELF is decreased by FF 
target heading when ADELTH is increased by AA 
target heading when ADELTH is decreased by AA 
target heading when AVS is increased by VV 
target heading when AVS is decreased by VV 


"UN Tor tub initial covariance matrix, 
= ] for only diagonal terms 


үл 5 саге сома еп сета Ба also n E 
state covariance matrix 


predicted state covariance matrix 

state vransitiengnatrıx 

PHl@transposed 

heading angle excitation in (rad/K see) 
target speed excitation in UA ЖЕ 
target rest frequency excitation (H, /Ksec)* 
predicted doppler shifted frequency at sensor N 
Stave excitation Matrix 

linearized observation Matrix 

HOB transposed 

Kalman Filter gain matrix 

GAIN transposed 

observation noise covariance matrix 
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XCUR 
XPRED 
XFIL 
TRE 
XMC(I,J) 


XVAR(I,J) 


XTAR (J ) 
YTAR(2) 


АИО) VIN Z ) 


RFIL(J),THFIL(J) 


state Vom before observation 
predicted state vector ( = PHI-XCUR) 
filtered State vector after observation 
residual error counter 

Monte Carlo mean of state I for time T(J) 


Monte Carlo variance of state I for time T(J) 
(See RCPA - XCPA filter) 


filtered X positicn of target at time T(J) 
filtered Y position of target at time T(J) 


лета Ре О а па А а: пробва ра Отранто 
Monte Carlo runs 


calculated target range and bearing from 
filtered states at time T(J) 
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SITO: PROGRAM. VARTABLES FOR X-Y FILTER 


Х(1) 
Х(2) 
x(3) 
469 
Х(5) 
XUF 


XDF 


SGXF 


SGVXA 


The other terms 


TT 
SNEWXV, SNEWXS, 
SNEWYV, SNEWYS 
HDING 


DE 


A POSICION 

X velocity 

Кров расом 

Y velocity 

rest frequency 

X refers to which state, U refers to up or 
mereasessancer reriersäte whicnhwariablegs 
changed. F is frequency term ADELF. Thus 
РА и они ето а с а гес“ акта position 


due to an increase in ADELF by FF 


celculatedis initial position due toma 
decrease in ADELF by FF 


average of the change due to increasing 
ADELE and the change due to decreasing ADZLF 
by AA 

average of the change due to increasing 


ADELTH and the change due to decreasing 
ADELTH py AA 


follow the same pattern 


time interval between measurements 

Monte Carlo variances and standard deviations 
се На DoSi viens Mmeieorated coordinave 
systeni 


calculated initial target heading 


predicted target bearing from sensor l in 
radians . 


angle residual for sensor 1 Wn radians 


frequency residual for sensor K in ша 


D 





RERROR(J ) 
SEED Qe) 
HDFIL(J) 
RTEMP 
XYVAR(I,J) 
XYVAR(Z,J) 
XYA(J) 


VXVYA(CJ ) 


true range error for time T(J) i.e. distance 
ronald ene dinos ton to true position 


computed target speed from filtered X and 
Y velocities at time T(J) 


computed target heading from filtered x 
and Y velocities at time T(J) 


computed target range from filtered X and 
XY positions 


Monte Carlo covariance of X and Y filter 
positions at time T(J) 


Monte Carlo covariance of X and Y filter 
И meos еј 


angle of Monte Carlo probability ellipse 
"or position variances 


angle of Monte Carlo probability ellipse 
for velocity variances 





LIST OF PROGRAM VARIABLES FOR R з E AR 
ES) range to target at CPA from sensor 1 
DC om range of target to CPA 
X(3) target speed 
LE ‘target heading 
A rest frequency 
REX exact value of Range to CPA from sensor 1 
XEX exact value of Range to CPA from target 
NDIFA number of angle and frequency sensors 
NLOFA number of frequency sensors 
NBU total number of sensors used 
W angular rotation parameter around sensor і 

(= +1 counterclockwise, = -1 clockwise) 

ВСРАТ initial filter state X(1) | 
XCPAI initial filter state X(2) 


Initial covariance terms are defined in a similar manner as 
mich (ET Геп гс а Iis the initial R due to an 

Е ы СРА 

increase in Avs by VV. 


DELTAT time interval between measurements 

D(1) distance from sensoraıto sensor Е 

RE (Su bearing from sensor 1 to sensor I 

Berne predicted range at CPA Tro sensor I 

XCPA(I) Predieted range to ОРА оТ target for sensor п 
RANGE(I) predicted range from target to sensor I 
AO predicted bearing of target from sensor 1 

RES 1 ange Ce L UAL Tor Sensor 1 


H 
л 
(АЈ 





RES 2 


RES 3, RES 4 


AVRESA 


STDRAZ 


АУКЕТ 


STFIZ 
ICOUNT 
IEND 
XMC(T,J), 


XMC(Z,J) 


XVAR(I,J), 
XVAR(Z,J) 


XYVAR(I,J) 


VARZ T) 


frequency residual for sensor 1 


angle and frequency residual depending on 
number and type of sensors 


statistical mean of RESI for a single run 


statistical standard deviation of RES1 for 
a Single run 


statistical mean of RES2 for a single run 


statistical variance of RES? for a single 
run 


number of times filter has been reinitialized 
due to residual error counter IRE 


maximum number of times a. reinitialization 
will be tried 


Domea іс mean ог сагвес> Х апа Y position 
at time T(J) 


Monte Carlo variance of target X and Y 
position at time T(J) 


Monte Carlo covariance of X and Y position 
atime. T(J) 


Monte Carlo covariance of target speed and 
heading at time T(J) 
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Flow Chart for Х-Ү 
Filter Simulation Program 


| | START 








READ IXA,IXF, 
NSPOT ,JJQQ 


READ TARGET DATA 


miese 










FIX M,NUM,TINT 


FIX SENSOR 
POSITION 













{ c = 
COMPUTE EXACT 
DATA FOR EACH 
SENSOR 









4 WRITE EXACT 
m DATA 





JQ=JQ+1 == 
А CALL GAUSS 
ADD NOISE TO 
SIMULATE INPUT 






E C» ree 







WRITE 
SIMULATED 
DATA 


(OPTIONAL ) 





^ 
b 


Sg 
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Flow Chart for X-Y Filter, 
Filter Program 


qme amus 







сӯ 


SET AVSK 
SEL LU 


| NO (KJ ,1) 
< -2(1,1)>2 x SA 


COMPUTE 
SIN(0c-0, uc) 
AVSK = P 

AVSK+2 









COMPUTE INITIAL 
HEADING, RANGE 
AND REST FREQ 


El 
NO TS THERE 
——<ÓMORE THAN 1 
SENSOR" 
YES 


COMPUTE 
NEW RANGE | 


7 
COMPUTE 
INITIAL FIL 








(OPTIONAL) 
———Pp!WRITE INITIAL 
STATES 





7 
INITIALIZE THE 


COVARIANCE 
MATRIX 
















WRITE INITIAL 
| COVARIANCE 
| MATRIX o 


(OPTIONAL) „, 


TRANSFER STÄTES 
TO FIRST. MEASURE- 
MENT TIME 








ETTE 





А 
COMPUTE 
EXCITATION 
MATRIX 


COMPUTE 
PREDICTED 
STATES 


Co LLL) EMIT 


COMPUTE 
PREDICTED 
COVARIANCE 
MATRIX 
























7 
COMPUTE | 
OBSERVATION 


MATRIX 


COMPUTE 
FILTER 
GAINS 


5? 
CALCULATE 
RESIDUALS: 

ANGLE-RESA 


FREQ-RESF 


NO AS м 
| OR 


RESF « 2xSF woe 
| YES 












£A 


INCREMENT | 
RESIDUAL 
COUNTER 






















COMPUTE 
FILTERED 










| WRITE CURRENT 
(OPTIONAL) | PREDICTED AND 
ESTIMATED 
STATES 








v. 


UPDATE 
COVARIANCE 


ЕЈ 








(OPTIONAL) | PLDT TRUE 


¡ TRACK AND 
| FILTER TRACK 






COMPUTE 
MÓNTE CARLO 
STATISTICS 











ROTATE 
POSITION ERR 


ELLIPSES WRITE 
MONTE CARLO 
ba A STATISTICS 





PELOT TRUE 
TRACK AND 
MONTE CARLO 
MEAN TRACK 







WRITE ANGLE 
AND NEW 
VARTANCES 


> 
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